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

Merge branch 'develop' into feature/timing_scripts_build 

Conflicts:
	gtsam_unstable/CMakeLists.txt
release/4.3a0
Richard Roberts 2014-06-21 08:55:49 -07:00
parent fe235b1209 921a18ae27
commit 946cc5338a
72 changed files with 6401 additions and 1608 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2280
.cproject
View File

File diff suppressed because it is too large Load Diff

4
CMakeLists.txt
View File

@ -34,7 +34,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR})
message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ")
endif()
# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout)
# See whether gtsam_unstable is available (it will be present only if we're using a git checkout)
if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable")
set(GTSAM_UNSTABLE_AVAILABLE 1)
else()
@ -93,7 +93,7 @@ if(MSVC)
# Disable autolinking
if(NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_NO_LIB)
add_definitions(-DBOOST_ALL_DYN_LINK)
add_definitions(-DBOOST_ALL_DYN_LINK)
endif()
endif()

2
examples/Pose2SLAMExample_g2o.cpp
View File

@ -31,7 +31,7 @@ int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = "../../examples/Data/noisyToyGraph.txt";
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];

3
examples/Pose2SLAMExample_graph.cpp
View File

@ -32,7 +32,8 @@ int main (int argc, char** argv) {
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model);
string graph_file = findExampleDataFile("w100.graph");
boost::tie(graph, initial) = load2D(graph_file, model);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses

2
examples/Pose2SLAMExample_lago.cpp
View File

@ -32,7 +32,7 @@ int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = "../../examples/Data/noisyToyGraph.txt";
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];

2
examples/Pose2SLAMwSPCG.cpp
View File

@ -104,7 +104,7 @@ int main(int argc, char** argv) {
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();

11
examples/RangeISAMExample_plaza2.cpp
View File

@ -40,6 +40,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
@ -59,9 +60,8 @@ namespace NM = gtsam::noiseModel;
typedef pair<double, Pose2> TimedOdometry;
list<TimedOdometry> readOdometry() {
list<TimedOdometry> odometryList;
ifstream is("../../examples/Data/Plaza2_DR.txt");
if (!is)
throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found");
string data_file = findExampleDataFile("Plaza2_DR.txt");
ifstream is(data_file.c_str());
while (is) {
double t, distance_traveled, delta_heading;
@ -78,9 +78,8 @@ list<TimedOdometry> readOdometry() {
typedef boost::tuple<double, size_t, double> RangeTriple;
vector<RangeTriple> readTriples() {
vector<RangeTriple> triples;
ifstream is("../../examples/Data/Plaza2_TD.txt");
if (!is)
throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found");
string data_file = findExampleDataFile("Plaza2_TD.txt");
ifstream is(data_file.c_str());
while (is) {
double t, sender, receiver, range;

25
examples/StereoVOExample.cpp
View File

@ -24,47 +24,48 @@
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose = Pose3();
graph.push_back(NonlinearEquality<Pose3>(1, first_pose));
Pose3 first_pose;
graph.push_back(NonlinearEquality<Pose3>(1, Pose3()));
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
//create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate = Values();
Values initial_estimate;
//create and add iniital estimates
initial_estimate.insert(1, first_pose);
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
initial_estimate.insert(3, Point3(1, 1, 5));
initial_estimate.insert(4, Point3(-1, 1, 5));
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
@ -72,4 +73,4 @@ int main(int argc, char** argv){
result.print("Final result:\n");
return 0;
}
}

42
gtsam.h
View File

@ -1481,9 +1481,7 @@ class GaussianISAM {
#include <gtsam/linear/IterativeSolver.h>
virtual class IterativeOptimizationParameters {
string getKernel() const ;
string getVerbosity() const;
void setKernel(string s) ;
void setVerbosity(string s) ;
void print() const;
};
@ -1516,7 +1514,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
void print() const;
};
class SubgraphSolver {
virtual class SubgraphSolver {
SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters &parameters, const gtsam::Ordering& ordering);
gtsam::VectorValues optimize() const;
@ -1855,12 +1853,12 @@ virtual class NonlinearOptimizerParams {
void setLinearSolverType(string solver);
void setOrdering(const gtsam::Ordering& ordering);
void setIterativeParams(const gtsam::SubgraphSolverParameters &params);
void setIterativeParams(gtsam::IterativeOptimizationParameters* params);
bool isMultifrontal() const;
bool isSequential() const;
bool isCholmod() const;
bool isCG() const;
bool isIterative() const;
};
bool checkConvergence(double relativeErrorTreshold,
@ -2199,6 +2197,25 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
void serialize() const;
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<POSE, LANDMARK, CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor);
SmartProjectionPoseFactor(double rankTol);
SmartProjectionPoseFactor();
void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i,
const CALIBRATION* K_i);
// enabling serialization functionality
//void serialize() const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>
template<POSE, LANDMARK>
@ -2301,7 +2318,8 @@ virtual class ConstantBias : gtsam::Value {
#include <gtsam/navigation/ImuFactor.h>
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
@ -2339,6 +2357,15 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
@ -2352,8 +2379,7 @@ class CombinedImuFactorPreintegratedMeasurements {
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::noiseModel::Base* model);
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;

166
gtsam/geometry/Cal3DS2.cpp
View File

@ -25,7 +25,7 @@ namespace gtsam {
/* ************************************************************************* */
Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const {
@ -34,32 +34,64 @@ Matrix Cal3DS2::K() const {
/* ************************************************************************* */
Vector Cal3DS2::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_);
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */
void Cal3DS2::print(const std::string& s) const {
gtsam::print(K(), s + ".K");
gtsam::print(Vector(k()), s + ".k");
void Cal3DS2::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
}
/* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol)
fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
return false;
return true;
}
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
D << DR1, DK * DR2;
return D;
}
// parameters
const double fx = fx_, fy = fy_, s = s_;
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
@ -68,40 +100,29 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1 * rr + k2 * r4;
const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx);
const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy);
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
const double pnx = g*x + dx;
const double pny = g*y + dy;
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Inlined derivative for calibration
if (H1) {
*H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
}
// Inlined derivative for points
if (H2) {
const double dr_dx = 2. * x;
const double dr_dy = 2. * y;
const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx;
const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy;
// Radial and tangential distortion applied
const double pnx = g * x + dx;
const double pny = g * y + dy;
const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x);
const double dDx_dy = 2. * k3 * x + k4 * dr_dy;
const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
Eigen::Matrix<double, 2, 2> DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy);
Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
*H2 = DK * DR;
}
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_);
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
@ -118,14 +139,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for ( iteration = 0; iteration < maxIterations; ++iteration ) {
if ( uncalibrate(pn).distance(pi) <= tol ) break;
const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1+k1_*rr+k2_*rr*rr);
const double dx = 2*k3_*xy + k4_*(rr+2*xx);
const double dy = 2*k4_*xy + k3_*(rr+2*yy);
pn = (invKPi - Point2(dx,dy))/g;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
@ -136,47 +157,28 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
/* ************************************************************************* */
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
//const double fx = fx_, fy = fy_, s = s_;
const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
//const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double dr_dx = 2*x;
const double dr_dy = 2*y;
const double r4 = rr*rr;
const double g = 1 + k1*rr + k2*r4;
const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx;
const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy;
// Dx = 2*k3*xy + k4*(rr+2*xx);
// Dy = 2*k4*xy + k3*(rr+2*yy);
const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x);
const double dDx_dy = 2*k3*x + k4*dr_dy;
const double dDy_dx = 2*k4*y + k3*dr_dx;
const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy);
return DK * DR;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr*rr;
const double fx = fx_, fy = fy_, s = s_;
const double g = (1+k1_*rr+k2_*r4);
const double dx = 2*k3_*xy + k4_*(rr+2*xx);
const double dy = 2*k4_*xy + k3_*(rr+2*yy);
const double pnx = g*x + dx;
const double pny = g*y + dy;
return (Matrix(2, 9) <<
pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) );
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
/* ************************************************************************* */

47
gtsam/geometry/Cal3DS2.h
View File

@ -27,6 +27,15 @@ namespace gtsam {
* @brief Calibration of a camera with radial distortion
* @addtogroup geometry
* \nosubgrouping
*
* Uses same distortionmodel as OpenCV, with
* http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
* but using only k1,k2,p1, and p2 coefficients.
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* rr = Pn.x^2 + Pn.y^2
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
@ -34,28 +43,22 @@ protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double k3_, k4_ ; // tangential distortion
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
// rr = Pn.x^2 + Pn.y^2
// \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
// k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
// pi = K*pn
double p1_, p2_ ; // tangential distortion
public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); }
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double k3 = 0.0, double k4 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
/// @}
/// @name Advanced Constructors
@ -92,18 +95,30 @@ public:
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
/// Second distortion coefficient
inline double k2() const { return k2_;}
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/**
* convert intrinsic coordinates xy to image coordinates uv
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Conver a pixel coordinate to ideal coordinate
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
@ -148,8 +163,8 @@ private:
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(k3_);
ar & BOOST_SERIALIZATION_NVP(k4_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
}

2
gtsam/geometry/Cal3Unified.cpp
View File

@ -43,7 +43,7 @@ void Cal3Unified::print(const std::string& s) const {
bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol ||
fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol ||
fabs(xi_ - K.xi_) > tol)
return false;
return true;

19
gtsam/geometry/Cal3Unified.h
View File

@ -31,6 +31,14 @@ namespace gtsam {
* @brief Calibration of a omni-directional camera with mirror + lens radial distortion
* @addtogroup geometry
* \nosubgrouping
*
* Similar to Cal3DS2, does distortion but has additional mirror parameter xi
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
* rr = Pn.x^2 + Pn.y^2
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
@ -41,13 +49,6 @@ private:
double xi_; // mirror parameter
// K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
// Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
// rr = Pn.x^2 + Pn.y^2
// \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
// k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
// pi = K*pn
public:
//Matrix K() const ;
//Eigen::Vector4d k() const { return Base::k(); }
@ -60,8 +61,8 @@ public:
Cal3Unified() : Base(), xi_(0) {}
Cal3Unified(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) :
Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
/// @}
/// @name Advanced Constructors

4
gtsam/geometry/tests/testCal3DS2.cpp
View File

@ -60,6 +60,8 @@ TEST( Cal3DS2, Duncalibrate1)
K.uncalibrate(p, computed, boost::none);
Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
Matrix separate = K.D2d_calibration(p);
CHECK(assert_equal(numerical,separate,1e-5));
}
/* ************************************************************************* */
@ -68,6 +70,8 @@ TEST( Cal3DS2, Duncalibrate2)
Matrix computed; K.uncalibrate(p, boost::none, computed);
Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7);
CHECK(assert_equal(numerical,computed,1e-5));
Matrix separate = K.D2d_intrinsic(p);
CHECK(assert_equal(numerical,separate,1e-5));
}
/* ************************************************************************* */

11
gtsam/inference/Ordering.h
View File

@ -17,9 +17,11 @@
#pragma once
#include <algorithm>
#include <vector>
#include <boost/assign/list_inserter.hpp>
#include <gtsam/base/FastSet.h>
#include <gtsam/inference/Key.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/inference/FactorGraph.h>
@ -135,6 +137,15 @@ namespace gtsam {
static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex,
const FastMap<Key, int>& groups);
/// Return a natural Ordering. Typically used by iterative solvers
template <class FACTOR>
static Ordering Natural(const FactorGraph<FACTOR> &fg) {
FastSet<Key> src = fg.keys();
std::vector<Key> keys(src.begin(), src.end());
std::stable_sort(keys.begin(), keys.end());
return Ordering(keys);
}
/// @}
/// @name Testable @{

49
gtsam/linear/ConjugateGradientSolver.cpp Normal file
View File

@ -0,0 +1,49 @@
/*
* ConjugateGradientSolver.cpp
*
* Created on: Jun 4, 2014
* Author: ydjian
*/
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
using namespace std;
namespace gtsam {
/*****************************************************************************/
void ConjugateGradientParameters::print(ostream &os) const {
Base::print(os);
cout << "ConjugateGradientParameters" << endl
<< "minIter: " << minIterations_ << endl
<< "maxIter: " << maxIterations_ << endl
<< "resetIter: " << reset_ << endl
<< "eps_rel: " << epsilon_rel_ << endl
<< "eps_abs: " << epsilon_abs_ << endl;
}
/*****************************************************************************/
std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) {
std::string s;
switch (value) {
case ConjugateGradientParameters::GTSAM: s = "GTSAM" ; break;
default: s = "UNDEFINED" ; break;
}
return s;
}
/*****************************************************************************/
ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "GTSAM") return ConjugateGradientParameters::GTSAM;
/* default is SBM */
return ConjugateGradientParameters::GTSAM;
}
}

106
gtsam/linear/ConjugateGradientSolver.h
View File

@ -12,14 +12,15 @@
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <iosfwd>
namespace gtsam {
/**
* parameters for the conjugate gradient method
*/
class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters {
class ConjugateGradientParameters : public IterativeOptimizationParameters {
public:
typedef IterativeOptimizationParameters Base;
typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
@ -30,14 +31,23 @@ public:
double epsilon_rel_; ///< threshold for relative error decrease
double epsilon_abs_; ///< threshold for absolute error decrease
ConjugateGradientParameters()
: minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), epsilon_abs_(1e-3){}
/* Matrix Operation Kernel */
enum BLASKernel {
GTSAM = 0, ///< Jacobian Factor Graph of GTSAM
} blas_kernel_ ;
ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, double epsilon_rel, double epsilon_abs)
: minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs){}
ConjugateGradientParameters()
: minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3),
epsilon_abs_(1e-3), blas_kernel_(GTSAM) {}
ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset,
double epsilon_rel, double epsilon_abs, BLASKernel blas)
: minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset),
epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {}
ConjugateGradientParameters(const ConjugateGradientParameters &p)
: Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_) {}
: Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_),
epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {}
/* general interface */
inline size_t minIterations() const { return minIterations_; }
@ -61,15 +71,81 @@ public:
inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
virtual void print() const {
Base::print();
std::cout << "ConjugateGradientParameters" << std::endl
<< "minIter: " << minIterations_ << std::endl
<< "maxIter: " << maxIterations_ << std::endl
<< "resetIter: " << reset_ << std::endl
<< "eps_rel: " << epsilon_rel_ << std::endl
<< "eps_abs: " << epsilon_abs_ << std::endl;
}
void print() const { Base::print(); }
virtual void print(std::ostream &os) const;
static std::string blasTranslator(const BLASKernel k) ;
static BLASKernel blasTranslator(const std::string &s) ;
};
/*********************************************************************************************/
/*
* A template of linear preconditioned conjugate gradient method.
* System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v,
* rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y)
* Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book.
*/
template <class S, class V>
V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters &parameters) {
V estimate, residual, direction, q1, q2;
estimate = residual = direction = q1 = q2 = initial;
system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
system.rightPrecondition(residual, direction);/* d = S^{-1} r */
double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta;
const size_t iMaxIterations = parameters.maxIterations(),
iMinIterations = parameters.minIterations(),
iReset = parameters.reset() ;
const double threshold = std::max(parameters.epsilon_abs(),
parameters.epsilon() * parameters.epsilon() * currentGamma);
if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
std::cout << "[PCG] epsilon = " << parameters.epsilon()
<< ", max = " << parameters.maxIterations()
<< ", reset = " << parameters.reset()
<< ", ||r0||^2 = " << currentGamma
<< ", threshold = " << threshold << std::endl;
size_t k;
for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) {
if ( k % iReset == 0 ) {
system.residual(estimate, q1); /* q1 = b-Ax */
system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */
system.rightPrecondition(residual, direction); /* d = S^{-1} r */
currentGamma = system.dot(residual, residual);
}
system.multiply(direction, q1); /* q1 = A d */
alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */
system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */
system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */
system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */
prevGamma = currentGamma;
currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */
beta = currentGamma / prevGamma;
system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */
system.scal(beta, direction);
system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */
if (parameters.verbosity() >= ConjugateGradientParameters::ERROR )
std::cout << "[PCG] k = " << k
<< ", alpha = " << alpha
<< ", beta = " << beta
<< ", ||r||^2 = " << currentGamma
<< std::endl;
}
if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY )
std::cout << "[PCG] iterations = " << k
<< ", ||r||^2 = " << currentGamma
<< std::endl;
return estimate;
}
}

14
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -54,6 +54,20 @@ namespace gtsam {
return keys;
}
/* ************************************************************************* */
std::map<Key, size_t> GaussianFactorGraph::getKeyDimMap() const {
map<Key, size_t> spec;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) {
for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) {
map<Key,size_t>::iterator it2 = spec.find(*it);
if ( it2 == spec.end() ) {
spec.insert(make_pair(*it, gf->getDim(it)));
}
}
}
return spec;
}
/* ************************************************************************* */
vector<size_t> GaussianFactorGraph::getkeydim() const {
// First find dimensions of each variable

3
gtsam/linear/GaussianFactorGraph.h
View File

@ -138,6 +138,9 @@ namespace gtsam {
typedef FastSet<Key> Keys;
Keys keys() const;
/* return a map of (Key, dimension) */
std::map<Key, size_t> getKeyDimMap() const;
std::vector<size_t> getkeydim() const;
/** unnormalized error */

118
gtsam/linear/IterativeSolver.cpp
View File

@ -6,25 +6,42 @@
*/
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <string>
using namespace std;
namespace gtsam {
std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); }
std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); }
void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); }
/*****************************************************************************/
string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "CG") return IterativeOptimizationParameters::CG;
/* default is cg */
else return IterativeOptimizationParameters::CG;
/*****************************************************************************/
void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); }
/*****************************************************************************/
void IterativeOptimizationParameters::print() const {
print(cout);
}
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
/*****************************************************************************/
void IterativeOptimizationParameters::print(ostream &os) const {
os << "IterativeOptimizationParameters:" << endl
<< "verbosity: " << verbosityTranslator(verbosity_) << endl;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) {
p.print(os);
return os;
}
/*****************************************************************************/
IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) {
string s = src; boost::algorithm::to_upper(s);
if (s == "SILENT") return IterativeOptimizationParameters::SILENT;
else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY;
else if (s == "ERROR") return IterativeOptimizationParameters::ERROR;
@ -32,18 +49,85 @@ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verb
else return IterativeOptimizationParameters::SILENT;
}
std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) {
if ( k == CG ) return "CG";
else return "UNKNOWN";
}
std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) {
/*****************************************************************************/
string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) {
if (verbosity == SILENT) return "SILENT";
else if (verbosity == COMPLEXITY) return "COMPLEXITY";
else if (verbosity == ERROR) return "ERROR";
else return "UNKNOWN";
}
/*****************************************************************************/
VectorValues IterativeSolver::optimize(
const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> keyInfo,
boost::optional<const std::map<Key, Vector>&> lambda)
{
return optimize(
gfg,
keyInfo ? *keyInfo : KeyInfo(gfg),
lambda ? *lambda : std::map<Key,Vector>());
}
/*****************************************************************************/
VectorValues IterativeSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda)
{
return optimize(gfg, keyInfo, lambda, keyInfo.x0());
}
/****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering)
: ordering_(ordering) {
initialize(fg);
}
/****************************************************************************/
KeyInfo::KeyInfo(const GaussianFactorGraph &fg)
: ordering_(Ordering::Natural(fg)) {
initialize(fg);
}
/****************************************************************************/
void KeyInfo::initialize(const GaussianFactorGraph &fg){
const map<Key, size_t> colspec = fg.getKeyDimMap();
const size_t n = ordering_.size();
size_t start = 0;
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t key = ordering_[i];
const size_t dim = colspec.find(key)->second;
insert(make_pair(key, KeyInfoEntry(i, dim, start)));
start += dim ;
}
numCols_ = start;
}
/****************************************************************************/
vector<size_t> KeyInfo::colSpec() const {
std::vector<size_t> result(size(), 0);
BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
result[item.second.index()] = item.second.dim();
}
return result;
}
/****************************************************************************/
VectorValues KeyInfo::x0() const {
VectorValues result;
BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) {
result.insert(item.first, Vector::Zero(item.second.dim()));
}
return result;
}
/****************************************************************************/
Vector KeyInfo::x0vector() const {
return Vector::Zero(numCols_);
}
}

111
gtsam/linear/IterativeSolver.h
View File

@ -11,17 +11,27 @@
#pragma once
#include <gtsam/base/Vector.h>
#include <gtsam/global_includes.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
#include <boost/optional/optional.hpp>
#include <boost/none.hpp>
#include <iosfwd>
#include <map>
#include <string>
#include <iostream>
#include <vector>
namespace gtsam {
// Forward declarations
class VectorValues;
class KeyInfo;
class KeyInfoEntry;
class GaussianFactorGraph;
class Values;
class VectorValues;
/************************************************************************************/
/**
* parameters for iterative linear solvers
*/
@ -30,55 +40,100 @@ namespace gtsam {
public:
typedef boost::shared_ptr<IterativeOptimizationParameters> shared_ptr;
enum Kernel { CG = 0 } kernel_ ; ///< Iterative Method Kernel
enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_;
public:
IterativeOptimizationParameters(const IterativeOptimizationParameters &p)
: kernel_(p.kernel_), verbosity_(p.verbosity_) {}
IterativeOptimizationParameters(const Kernel kernel = CG, const Verbosity verbosity = SILENT)
: kernel_(kernel), verbosity_(verbosity) {}
IterativeOptimizationParameters(Verbosity v = SILENT)
: verbosity_(v) {}
virtual ~IterativeOptimizationParameters() {}
/* general interface */
inline Kernel kernel() const { return kernel_; }
/* utility */
inline Verbosity verbosity() const { return verbosity_; }
/* matlab interface */
std::string getKernel() const ;
std::string getVerbosity() const;
void setKernel(const std::string &s) ;
void setVerbosity(const std::string &s) ;
virtual void print() const {
std::cout << "IterativeOptimizationParameters" << std::endl
<< "kernel: " << kernelTranslator(kernel_) << std::endl
<< "verbosity: " << verbosityTranslator(verbosity_) << std::endl;
}
/* matlab interface */
void print() const ;
/* virtual print function */
virtual void print(std::ostream &os) const ;
/* for serialization */
friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p);
static Kernel kernelTranslator(const std::string &s);
static Verbosity verbosityTranslator(const std::string &s);
static std::string kernelTranslator(Kernel k);
static std::string verbosityTranslator(Verbosity v);
};
/************************************************************************************/
class GTSAM_EXPORT IterativeSolver {
public:
typedef boost::shared_ptr<IterativeSolver> shared_ptr;
IterativeSolver() {}
virtual ~IterativeSolver() {}
/* interface to the nonlinear optimizer */
virtual VectorValues optimize () = 0;
/* interface to the nonlinear optimizer, without metadata, damping and initial estimate */
VectorValues optimize (
const GaussianFactorGraph &gfg,
boost::optional<const KeyInfo&> = boost::none,
boost::optional<const std::map<Key, Vector>&> lambda = boost::none
);
/* interface to the nonlinear optimizer */
virtual VectorValues optimize (const VectorValues &initial) = 0;
/* interface to the nonlinear optimizer, without initial estimate */
VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda
);
/* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) = 0;
/* update interface to the nonlinear optimizer */
virtual void replaceFactors(const boost::shared_ptr<GaussianFactorGraph> &factorGraph, const double lambda) {}
};
/************************************************************************************/
/* Handy data structure for iterative solvers
* key to (index, dimension, colstart) */
class GTSAM_EXPORT KeyInfoEntry : public boost::tuple<size_t, size_t, size_t> {
public:
typedef boost::tuple<Key,size_t,Key> Base;
KeyInfoEntry(){}
KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {}
const size_t index() const { return this->get<0>(); }
const size_t dim() const { return this->get<1>(); }
const size_t colstart() const { return this->get<2>(); }
};
/************************************************************************************/
class GTSAM_EXPORT KeyInfo : public std::map<Key, KeyInfoEntry> {
public:
typedef std::map<Key, KeyInfoEntry> Base;
KeyInfo() : numCols_(0) {}
KeyInfo(const GaussianFactorGraph &fg);
KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering);
std::vector<size_t> colSpec() const ;
VectorValues x0() const;
Vector x0vector() const;
inline size_t numCols() const { return numCols_; }
inline const Ordering & ordering() const { return ordering_; }
protected:
void initialize(const GaussianFactorGraph &fg);
Ordering ordering_;
size_t numCols_;
};
}

201
gtsam/linear/PCGSolver.cpp Normal file
View File

@ -0,0 +1,201 @@
/*
* PCGSolver.cpp
*
* Created on: Feb 14, 2012
* Author: ydjian
*/
#include <gtsam/linear/GaussianFactorGraph.h>
//#include <gtsam/inference/FactorGraph-inst.h>
//#include <gtsam/linear/FactorGraphUtil-inl.h>
//#include <gtsam/linear/JacobianFactorGraph.h>
//#include <gtsam/linear/LSPCGSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
//#include <gtsam/linear/SuiteSparseUtil.h>
//#include <gtsam/linear/ConjugateGradientMethod-inl.h>
//#include <gsp2/gtsam-interface-sbm.h>
//#include <ydjian/tool/ThreadSafeTimer.h>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <stdexcept>
using namespace std;
namespace gtsam {
/*****************************************************************************/
void PCGSolverParameters::print(ostream &os) const {
Base::print(os);
os << "PCGSolverParameters:" << endl;
preconditioner_->print(os);
}
/*****************************************************************************/
PCGSolver::PCGSolver(const PCGSolverParameters &p) {
preconditioner_ = createPreconditioner(p.preconditioner_);
}
/*****************************************************************************/
VectorValues PCGSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial)
{
/* build preconditioner */
preconditioner_->build(gfg, keyInfo, lambda);
/* apply pcg */
const Vector sol = preconditionedConjugateGradient<GaussianFactorGraphSystem, Vector>(
GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda),
initial.vector(keyInfo.ordering()), parameters_);
return buildVectorValues(sol, keyInfo);
}
/*****************************************************************************/
GaussianFactorGraphSystem::GaussianFactorGraphSystem(
const GaussianFactorGraph &gfg,
const Preconditioner &preconditioner,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda)
: gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {}
/*****************************************************************************/
void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const {
/* implement b-Ax, assume x and r are pre-allocated */
/* reset r to b */
getb(r);
/* substract A*x */
Vector Ax = Vector::Zero(r.rows(), 1);
multiply(x, Ax);
r -= Ax ;
}
/*****************************************************************************/
void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const {
/* implement Ax, assume x and Ax are pre-allocated */
/* reset y */
Ax.setZero();
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
/* accumulate At A x*/
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const Matrix Ai = jf->getA(it);
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
Ax.segment(entry.colstart(), entry.dim())
+= Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim()));
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate H x */
/* use buffer to avoid excessive table lookups */
const size_t sz = hf->size();
vector<Vector> y;
y.reserve(sz);
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
/* initialize y to zeros */
y.push_back(zero(hf->getDim(it)));
}
for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) {
/* retrieve the key mapping */
const KeyInfoEntry &entry = keyInfo_.find(*j)->second;
// xj is the input vector
const Vector xj = x.segment(entry.colstart(), entry.dim());
size_t idx = 0;
for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) {
if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj;
else y[idx] += hf->info(i, j).knownOffDiagonal() * xj;
}
}
/* accumulate to r */
for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) {
/* retrieve the key mapping */
const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second;
Ax.segment(entry.colstart(), entry.dim()) += y[i];
}
}
else {
throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
}
/*****************************************************************************/
void GaussianFactorGraphSystem::getb(Vector &b) const {
/* compute rhs, assume b pre-allocated */
/* reset */
b.setZero();
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
const Vector rhs = jf->getb();
/* accumulate At rhs */
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
/* this map lookup should be replaced */
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ;
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
/* accumulate g */
for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) {
const KeyInfoEntry &entry = keyInfo_.find(*it)->second;
b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it);
}
}
else {
throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
}
/**********************************************************************************/
void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.transposeSolve(x, y); }
/**********************************************************************************/
void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const
{ preconditioner_.solve(x, y); }
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v,
const Ordering &ordering,
const map<Key, size_t> & dimensions) {
VectorValues result;
DenseIndex offset = 0;
for ( size_t i = 0 ; i < ordering.size() ; ++i ) {
const Key &key = ordering[i];
map<Key, size_t>::const_iterator it = dimensions.find(key);
if ( it == dimensions.end() ) {
throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions");
}
const size_t dim = it->second;
result.insert(key, v.segment(offset, dim));
offset += dim;
}
return result;
}
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) {
VectorValues result;
BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) {
result.insert(item.first, v.segment(item.second.colstart(), item.second.dim()));
}
return result;
}
}

109
gtsam/linear/PCGSolver.h Normal file
View File

@ -0,0 +1,109 @@
/*
* PCGSolver.h
*
* Created on: Jan 31, 2012
* Author: Yong-Dian Jian
*/
#pragma once
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
#include <algorithm>
#include <iosfwd>
#include <map>
#include <string>
namespace gtsam {
class GaussianFactorGraph;
class KeyInfo;
class Preconditioner;
struct PreconditionerParameters;
/*****************************************************************************/
struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
typedef boost::shared_ptr<PCGSolverParameters> shared_ptr;
PCGSolverParameters() {}
virtual void print(std::ostream &os) const;
/* interface to preconditioner parameters */
inline const PreconditionerParameters& preconditioner() const {
return *preconditioner_;
}
boost::shared_ptr<PreconditionerParameters> preconditioner_;
};
/*****************************************************************************/
/* A virtual base class for the preconditioned conjugate gradient solver */
class GTSAM_EXPORT PCGSolver: public IterativeSolver {
public:
typedef IterativeSolver Base;
typedef boost::shared_ptr<PCGSolver> shared_ptr;
protected:
PCGSolverParameters parameters_;
boost::shared_ptr<Preconditioner> preconditioner_;
public:
/* Interface to initialize a solver without a problem */
PCGSolver(const PCGSolverParameters &p);
virtual ~PCGSolver() {}
using IterativeSolver::optimize;
virtual VectorValues optimize(const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo, const std::map<Key, Vector> &lambda,
const VectorValues &initial);
};
/*****************************************************************************/
class GTSAM_EXPORT GaussianFactorGraphSystem {
public:
GaussianFactorGraphSystem(const GaussianFactorGraph &gfg,
const Preconditioner &preconditioner, const KeyInfo &info,
const std::map<Key, Vector> &lambda);
const GaussianFactorGraph &gfg_;
const Preconditioner &preconditioner_;
const KeyInfo &keyInfo_;
const std::map<Key, Vector> &lambda_;
void residual(const Vector &x, Vector &r) const;
void multiply(const Vector &x, Vector& y) const;
void leftPrecondition(const Vector &x, Vector &y) const;
void rightPrecondition(const Vector &x, Vector &y) const;
inline void scal(const double alpha, Vector &x) const {
x *= alpha;
}
inline double dot(const Vector &x, const Vector &y) const {
return x.dot(y);
}
inline void axpy(const double alpha, const Vector &x, Vector &y) const {
y += alpha * x;
}
void getb(Vector &b) const;
};
/* utility functions */
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const Ordering &ordering,
const std::map<Key, size_t> & dimensions);
/**********************************************************************************/
VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo);
}

214
gtsam/linear/Preconditioner.cpp Normal file
View File

@ -0,0 +1,214 @@
/*
* Preconditioner.cpp
*
* Created on: Jun 2, 2014
* Author: ydjian
*/
#include <gtsam/inference/FactorGraph-inst.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/NoiseModel.h>
#include <boost/shared_ptr.hpp>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <vector>
using namespace std;
namespace gtsam {
/*****************************************************************************/
void PreconditionerParameters::print() const {
print(cout);
}
/***************************************************************************************/
void PreconditionerParameters::print(ostream &os) const {
os << "PreconditionerParameters" << endl
<< "kernel: " << kernelTranslator(kernel_) << endl
<< "verbosity: " << verbosityTranslator(verbosity_) << endl;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const PreconditionerParameters &p) {
p.print(os);
return os;
}
/***************************************************************************************/
PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "GTSAM") return PreconditionerParameters::GTSAM;
else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD;
/* default is cholmod */
else return PreconditionerParameters::CHOLMOD;
}
/***************************************************************************************/
PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "SILENT") return PreconditionerParameters::SILENT;
else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY;
else if (s == "ERROR") return PreconditionerParameters::ERROR;
/* default is default */
else return PreconditionerParameters::SILENT;
}
/***************************************************************************************/
std::string PreconditionerParameters::kernelTranslator(PreconditionerParameters::Kernel k) {
if ( k == GTSAM ) return "GTSAM";
if ( k == CHOLMOD ) return "CHOLMOD";
else return "UNKNOWN";
}
/***************************************************************************************/
std::string PreconditionerParameters::verbosityTranslator(PreconditionerParameters::Verbosity verbosity) {
if (verbosity == SILENT) return "SILENT";
else if (verbosity == COMPLEXITY) return "COMPLEXITY";
else if (verbosity == ERROR) return "ERROR";
else return "UNKNOWN";
}
/***************************************************************************************/
BlockJacobiPreconditioner::BlockJacobiPreconditioner()
: Base(), buffer_(0), bufferSize_(0), nnz_(0) {}
/***************************************************************************************/
BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); }
/***************************************************************************************/
void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const {
const size_t n = dims_.size();
double *ptr = buffer_, *dst = x.data();
std::copy(y.data(), y.data() + y.rows(), x.data());
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t d = dims_[i];
const size_t sz = d*d;
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
R.triangularView<Eigen::Upper>().solveInPlace(b);
dst += d;
ptr += sz;
}
}
/***************************************************************************************/
void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const {
const size_t n = dims_.size();
double *ptr = buffer_, *dst = x.data();
std::copy(y.data(), y.data() + y.rows(), x.data());
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t d = dims_[i];
const size_t sz = d*d;
const Eigen::Map<const Eigen::MatrixXd> R(ptr, d, d);
Eigen::Map<Eigen::VectorXd> b(dst, d, 1);
R.transpose().triangularView<Eigen::Upper>().solveInPlace(b);
dst += d;
ptr += sz;
}
}
/***************************************************************************************/
void BlockJacobiPreconditioner::build(
const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
const size_t n = keyInfo.size();
dims_ = keyInfo.colSpec();
/* prepare the buffer of block diagonals */
std::vector<Matrix> blocks; blocks.reserve(n);
/* allocate memory for the factorization of block diagonals */
size_t nnz = 0;
for ( size_t i = 0 ; i < n ; ++i ) {
const size_t dim = dims_[i];
blocks.push_back(Matrix::Zero(dim, dim));
// nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ;
nnz += dim*dim;
}
/* compute the block diagonal by scanning over the factors */
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Ai = jf->getA(it);
blocks[entry.index()] += (Ai.transpose() * Ai);
}
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) {
const KeyInfoEntry &entry = keyInfo.find(*it)->second;
const Matrix &Hii = hf->info(it, it).selfadjointView();
blocks[entry.index()] += Hii;
}
}
else {
throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor.");
}
}
/* if necessary, allocating the memory for cacheing the factorization results */
if ( nnz > bufferSize_ ) {
clean();
buffer_ = new double [nnz];
bufferSize_ = nnz;
}
nnz_ = nnz;
/* factorizing the blocks respectively */
double *ptr = buffer_;
for ( size_t i = 0 ; i < n ; ++i ) {
/* use eigen to decompose Di */
const Matrix R = blocks[i].llt().matrixL().transpose();
/* store the data in the buffer */
size_t sz = dims_[i]*dims_[i] ;
std::copy(R.data(), R.data() + sz, ptr);
/* advance the pointer */
ptr += sz;
}
}
/*****************************************************************************/
void BlockJacobiPreconditioner::clean() {
if ( buffer_ ) {
delete [] buffer_;
buffer_ = 0;
bufferSize_ = 0;
nnz_ = 0;
}
}
/***************************************************************************************/
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters) {
if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast<DummyPreconditionerParameters>(parameters) ) {
return boost::make_shared<DummyPreconditioner>();
}
else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast<BlockJacobiPreconditionerParameters>(parameters) ) {
return boost::make_shared<BlockJacobiPreconditioner>();
}
else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast<SubgraphPreconditionerParameters>(parameters) ) {
return boost::make_shared<SubgraphPreconditioner>(*subgraph);
}
throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type");
}
}

170
gtsam/linear/Preconditioner.h Normal file
View File

@ -0,0 +1,170 @@
/*
* Preconditioner.h
*
* Created on: Jun 2, 2014
* Author: ydjian
*/
#pragma once
#include <gtsam/base/Vector.h>
#include <boost/shared_ptr.hpp>
#include <iosfwd>
#include <map>
#include <string>
namespace gtsam {
class GaussianFactorGraph;
class KeyInfo;
class VectorValues;
/* parameters for the preconditioner */
struct GTSAM_EXPORT PreconditionerParameters {
typedef boost::shared_ptr<PreconditionerParameters> shared_ptr;
enum Kernel { /* Preconditioner Kernel */
GTSAM = 0,
CHOLMOD /* experimental */
} kernel_ ;
enum Verbosity {
SILENT = 0,
COMPLEXITY = 1,
ERROR = 2
} verbosity_ ;
PreconditionerParameters(): kernel_(GTSAM), verbosity_(SILENT) {}
PreconditionerParameters(const PreconditionerParameters &p) : kernel_(p.kernel_), verbosity_(p.verbosity_) {}
virtual ~PreconditionerParameters() {}
/* general interface */
inline Kernel kernel() const { return kernel_; }
inline Verbosity verbosity() const { return verbosity_; }
void print() const ;
virtual void print(std::ostream &os) const ;
static Kernel kernelTranslator(const std::string &s);
static Verbosity verbosityTranslator(const std::string &s);
static std::string kernelTranslator(Kernel k);
static std::string verbosityTranslator(Verbosity v);
/* for serialization */
friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
};
/* PCG aims to solve the problem: A x = b by reparametrizing it as
* S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M
* The goal of this class is to provide a general interface to all preconditioners */
class GTSAM_EXPORT Preconditioner {
public:
typedef boost::shared_ptr<Preconditioner> shared_ptr;
typedef std::vector<size_t> Dimensions;
/* Generic Constructor and Destructor */
Preconditioner() {}
virtual ~Preconditioner() {}
/* Computation Interfaces */
/* implement x = S^{-1} y */
virtual void solve(const Vector& y, Vector &x) const = 0;
// virtual void solve(const VectorValues& y, VectorValues &x) const = 0;
/* implement x = S^{-T} y */
virtual void transposeSolve(const Vector& y, Vector& x) const = 0;
// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0;
// /* implement x = S^{-1} S^{-T} y */
// virtual void fullSolve(const Vector& y, Vector &x) const = 0;
// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0;
/* build/factorize the preconditioner */
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) = 0;
};
/*******************************************************************************************/
struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters {
typedef PreconditionerParameters Base;
typedef boost::shared_ptr<DummyPreconditionerParameters> shared_ptr;
DummyPreconditionerParameters() : Base() {}
virtual ~DummyPreconditionerParameters() {}
};
/*******************************************************************************************/
class GTSAM_EXPORT DummyPreconditioner : public Preconditioner {
public:
typedef Preconditioner Base;
typedef boost::shared_ptr<DummyPreconditioner> shared_ptr;
public:
DummyPreconditioner() : Base() {}
virtual ~DummyPreconditioner() {}
/* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const { x = y; }
// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; }
virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; }
// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; }
// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; }
// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; }
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) {}
};
/*******************************************************************************************/
struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters {
typedef PreconditionerParameters Base;
BlockJacobiPreconditionerParameters() : Base() {}
virtual ~BlockJacobiPreconditionerParameters() {}
};
/*******************************************************************************************/
class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner {
public:
typedef Preconditioner Base;
BlockJacobiPreconditioner() ;
virtual ~BlockJacobiPreconditioner() ;
/* Computation Interfaces for raw vector */
virtual void solve(const Vector& y, Vector &x) const;
virtual void transposeSolve(const Vector& y, Vector& x) const ;
// virtual void fullSolve(const Vector& y, Vector &x) const ;
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) ;
protected:
void clean() ;
std::vector<size_t> dims_;
double *buffer_;
size_t bufferSize_;
size_t nnz_;
};
/*********************************************************************************************/
/* factory method to create preconditioners */
boost::shared_ptr<Preconditioner> createPreconditioner(const boost::shared_ptr<PreconditionerParameters> parameters);
}

722
gtsam/linear/SubgraphPreconditioner.cpp
View File

@ -15,126 +15,646 @@
* @author: Frank Dellaert
*/
#include <gtsam/base/DSFVector.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <boost/foreach.hpp>
#include <boost/make_shared.hpp>
#include <boost/algorithm/string.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <fstream>
#include <iostream>
#include <numeric>
#include <queue>
#include <set>
#include <utility>
#include <vector>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
/* ************************************************************************* */
static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
if( !jf ) {
jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
}
result->push_back(jf);
}
return result;
}
/*****************************************************************************/
static std::vector<size_t> iidSampler(const vector<double> &weight, const size_t n) {
/* compute the sum of the weights */
const double sum = std::accumulate(weight.begin(), weight.end(), 0.0);
/* make a normalized and accumulated version of the weight vector */
const size_t m = weight.size();
vector<double> w; w.reserve(m);
for ( size_t i = 0 ; i < m ; ++i ) {
w.push_back(weight[i]/sum);
}
vector<double> acc(m);
std::partial_sum(w.begin(),w.end(),acc.begin());
/* iid sample n times */
vector<size_t> result; result.reserve(n);
const double denominator = (double)RAND_MAX;
for ( size_t i = 0 ; i < n ; ++i ) {
const double value = rand() / denominator;
/* binary search the interval containing "value" */
vector<double>::iterator it = std::lower_bound(acc.begin(), acc.end(), value);
size_t idx = it - acc.begin();
result.push_back(idx);
}
return result;
}
/*****************************************************************************/
vector<size_t> uniqueSampler(const vector<double> &weight, const size_t n) {
const size_t m = weight.size();
if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size");
vector<size_t> result;
size_t count = 0;
std::vector<bool> touched(m, false);
while ( count < n ) {
std::vector<size_t> localIndices; localIndices.reserve(n-count);
std::vector<double> localWeights; localWeights.reserve(n-count);
/* collect data */
for ( size_t i = 0 ; i < m ; ++i ) {
if ( !touched[i] ) {
localIndices.push_back(i);
localWeights.push_back(weight[i]);
}
result->push_back(jf);
}
return result;
}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar) :
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) {
}
/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
return *xbar_ + Rc1_->backSubstitute(y);
}
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e(y);
VectorValues x = this->x(y);
Errors e2 = Ab2()->gaussianErrors(x);
return 0.5 * (dot(e, e) + dot(e2,e2));
}
/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */
VectorValues v = VectorValues::Zero(x);
Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
return y + Rc1()->backSubstituteTranspose(v);
}
/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e(y);
VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */
Errors e2 = *Ab2() * x; /* A2*x */
e.splice(e.end(), e2);
return e;
}
/* ************************************************************************* */
// In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) {
*ei = y[i];
}
// Add A2 contribution
VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y
Ab2()->multiplyInPlace(x, ei); // use iterator version
}
/* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
Errors::const_iterator it = e.begin();
VectorValues y = zero();
for ( Key i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ;
transposeMultiplyAdd2(1.0,it,e.end(),y);
return y;
}
/* ************************************************************************* */
// y += alpha*A'*e
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++it ) {
const Vector& ei = *it;
axpy(alpha, ei, y[i]);
/* sampling and cache results */
vector<size_t> samples = iidSampler(localWeights, n-count);
BOOST_FOREACH ( const size_t &id, samples ) {
if ( touched[id] == false ) {
touched[id] = true ;
result.push_back(id);
if ( ++count >= n ) break;
}
}
transposeMultiplyAdd2(alpha, it, e.end(), y);
}
return result;
}
/****************************************************************************/
Subgraph::Subgraph(const std::vector<size_t> &indices) {
edges_.reserve(indices.size());
BOOST_FOREACH ( const size_t &idx, indices ) {
edges_.push_back(SubgraphEdge(idx, 1.0));
}
}
/****************************************************************************/
std::vector<size_t> Subgraph::edgeIndices() const {
std::vector<size_t> eid; eid.reserve(size());
BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) {
eid.push_back(edge.index_);
}
return eid;
}
/****************************************************************************/
void Subgraph::save(const std::string &fn) const {
std::ofstream os(fn.c_str());
boost::archive::text_oarchive oa(os);
oa << *this;
os.close();
}
/****************************************************************************/
Subgraph::shared_ptr Subgraph::load(const std::string &fn) {
std::ifstream is(fn.c_str());
boost::archive::text_iarchive ia(is);
Subgraph::shared_ptr subgraph(new Subgraph());
ia >> *subgraph;
is.close();
return subgraph;
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) {
if ( edge.weight() != 1.0 )
os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")";
else
os << edge.index() ;
return os;
}
/****************************************************************************/
std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) {
os << "Subgraph" << endl;
BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) {
os << e << ", " ;
}
return os;
}
/*****************************************************************************/
void SubgraphBuilderParameters::print() const {
print(cout);
}
/***************************************************************************************/
void SubgraphBuilderParameters::print(ostream &os) const {
os << "SubgraphBuilderParameters" << endl
<< "skeleton: " << skeletonTranslator(skeleton_) << endl
<< "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl
<< "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl
;
}
/*****************************************************************************/
ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) {
p.print(os);
return os;
}
/*****************************************************************************/
SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){
std::string s = src; boost::algorithm::to_upper(s);
if (s == "NATURALCHAIN") return NATURALCHAIN;
else if (s == "BFS") return BFS;
else if (s == "KRUSKAL") return KRUSKAL;
throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s);
return KRUSKAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) {
if ( w == NATURALCHAIN )return "NATURALCHAIN";
else if ( w == BFS ) return "BFS";
else if ( w == KRUSKAL )return "KRUSKAL";
else return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "EQUAL") return EQUAL;
else if (s == "RHS") return RHS_2NORM;
else if (s == "LHS") return LHS_FNORM;
else if (s == "RANDOM") return RANDOM;
throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s);
return EQUAL;
}
/****************************************************************/
std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) {
if ( w == EQUAL ) return "EQUAL";
else if ( w == RHS_2NORM ) return "RHS";
else if ( w == LHS_FNORM ) return "LHS";
else if ( w == RANDOM ) return "RANDOM";
else return "UNKNOWN";
}
/****************************************************************/
SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) {
std::string s = src; boost::algorithm::to_upper(s);
if (s == "SKELETON") return SKELETON;
// else if (s == "STRETCH") return STRETCH;
// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH;
throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s);
return SKELETON;
}
/****************************************************************/
std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) {
if ( w == SKELETON ) return "SKELETON";
// else if ( w == STRETCH ) return "STRETCH";
// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH";
else return "UNKNOWN";
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
const SubgraphBuilderParameters &p = parameters_;
switch (p.skeleton_) {
case SubgraphBuilderParameters::NATURALCHAIN:
return natural_chain(gfg);
break;
case SubgraphBuilderParameters::BFS:
return bfs(gfg);
break;
case SubgraphBuilderParameters::KRUSKAL:
return kruskal(gfg, ordering, w);
break;
default:
cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl;
break;
}
return vector<size_t>();
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( gf->size() == 1 ) {
result.push_back(idx);
}
idx++;
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const {
std::vector<size_t> result ;
size_t idx = 0;
BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
if ( gf->size() == 2 ) {
const Key k0 = gf->keys()[0], k1 = gf->keys()[1];
if ( (k1-k0) == 1 || (k0-k1) == 1 )
result.push_back(idx);
}
idx++;
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const {
const VariableIndex variableIndex(gfg);
/* start from the first key of the first factor */
Key seed = gfg[0]->keys()[0];
const size_t n = variableIndex.size();
/* each vertex has self as the predecessor */
std::vector<size_t> result;
result.reserve(n-1);
/* Initialize */
std::queue<size_t> q;
q.push(seed);
std::set<size_t> flags;
flags.insert(seed);
/* traversal */
while ( !q.empty() ) {
const size_t head = q.front(); q.pop();
BOOST_FOREACH ( const size_t id, variableIndex[head] ) {
const GaussianFactor &gf = *gfg[id];
BOOST_FOREACH ( const size_t key, gf.keys() ) {
if ( flags.count(key) == 0 ) {
q.push(key);
flags.insert(key);
result.push_back(id);
}
}
}
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const {
const VariableIndex variableIndex(gfg);
const size_t n = variableIndex.size();
const vector<size_t> idx = sort_idx(w) ;
/* initialize buffer */
vector<size_t> result;
result.reserve(n-1);
// container for acsendingly sorted edges
DSFVector D(n) ;
size_t count = 0 ; double sum = 0.0 ;
BOOST_FOREACH (const size_t id, idx) {
const GaussianFactor &gf = *gfg[id];
if ( gf.keys().size() != 2 ) continue;
const size_t u = ordering.find(gf.keys()[0])->second,
u_root = D.find(u),
v = ordering.find(gf.keys()[1])->second,
v_root = D.find(v) ;
if ( u_root != v_root ) {
D.merge(u_root, v_root) ;
result.push_back(id) ;
sum += w[id] ;
if ( ++count == n-1 ) break ;
}
}
return result;
}
/****************************************************************/
std::vector<size_t> SubgraphBuilder::sample(const std::vector<double> &weights, const size_t t) const {
return uniqueSampler(weights, t);
}
/****************************************************************/
Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const {
const SubgraphBuilderParameters &p = parameters_;
const Ordering inverse_ordering = Ordering::Natural(gfg);
const FastMap<Key, size_t> forward_ordering = inverse_ordering.invert();
const size_t n = inverse_ordering.size(), t = n * p.complexity_ ;
vector<double> w = weights(gfg);
const vector<size_t> tree = buildTree(gfg, forward_ordering, w);
/* sanity check */
if ( tree.size() != n-1 ) {
throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed ");
}
/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
/* down weight the tree edges to zero */
BOOST_FOREACH ( const size_t id, tree ) {
w[id] = 0.0;
}
/* decide how many edges to augment */
std::vector<size_t> offTree = sample(w, t);
vector<size_t> subgraph = unary(gfg);
subgraph.insert(subgraph.end(), tree.begin(), tree.end());
subgraph.insert(subgraph.end(), offTree.begin(), offTree.end());
return boost::make_shared<Subgraph>(subgraph);
}
/****************************************************************/
SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const
{
const size_t m = gfg.size() ;
Weights weight; weight.reserve(m);
BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) {
switch ( parameters_.skeletonWeight_ ) {
case SubgraphBuilderParameters::EQUAL:
weight.push_back(1.0);
break;
case SubgraphBuilderParameters::RHS_2NORM:
{
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
weight.push_back(jf->getb().norm());
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
weight.push_back(hf->linearTerm().norm());
}
}
break;
case SubgraphBuilderParameters::LHS_FNORM:
{
if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf) ) {
weight.push_back(std::sqrt(jf->getA().squaredNorm()));
}
else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast<HessianFactor>(gf) ) {
weight.push_back(std::sqrt(hf->information().squaredNorm()));
}
}
break;
case SubgraphBuilderParameters::RANDOM:
weight.push_back(std::rand()%100 + 1.0);
break;
default:
throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme ");
break;
}
}
return weight;
}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) :
parameters_(p) {}
/* ************************************************************************* */
SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) :
Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar),
b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) {
}
/* ************************************************************************* */
// x = xbar + inv(R1)*y
VectorValues SubgraphPreconditioner::x(const VectorValues& y) const {
return *xbar_ + Rc1_->backSubstitute(y);
}
/* ************************************************************************* */
double SubgraphPreconditioner::error(const VectorValues& y) const {
Errors e(y);
VectorValues x = this->x(y);
Errors e2 = Ab2()->gaussianErrors(x);
return 0.5 * (dot(e, e) + dot(e2,e2));
}
/* ************************************************************************* */
// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar),
VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const {
VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */
Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */
VectorValues v = VectorValues::Zero(x);
Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */
return y + Rc1()->backSubstituteTranspose(v);
}
/* ************************************************************************* */
// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y]
Errors SubgraphPreconditioner::operator*(const VectorValues& y) const {
Errors e(y);
VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */
Errors e2 = *Ab2() * x; /* A2*x */
e.splice(e.end(), e2);
return e;
}
/* ************************************************************************* */
// In-place version that overwrites e
void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const {
Errors::iterator ei = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) {
*ei = y[i];
}
// Add A2 contribution
VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y
Ab2()->multiplyInPlace(x, ei); // use iterator version
}
/* ************************************************************************* */
// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2
VectorValues SubgraphPreconditioner::operator^(const Errors& e) const {
Errors::const_iterator it = e.begin();
VectorValues y = zero();
for ( Key i = 0 ; i < y.size() ; ++i, ++it )
y[i] = *it ;
transposeMultiplyAdd2(1.0,it,e.end(),y);
return y;
}
/* ************************************************************************* */
// y += alpha*A'*e
void SubgraphPreconditioner::transposeMultiplyAdd
(double alpha, const Errors& e, VectorValues& y) const {
Errors::const_iterator it = e.begin();
for ( Key i = 0 ; i < y.size() ; ++i, ++it ) {
const Vector& ei = *it;
axpy(alpha, ei, y[i]);
}
transposeMultiplyAdd2(alpha, it, e.end(), y);
}
/* ************************************************************************* */
// y += alpha*inv(R1')*A2'*e2
void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha,
Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const {
// create e2 with what's left of e
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
Errors e2;
while (it != end) e2.push_back(*(it++));
// create e2 with what's left of e
// TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ?
Errors e2;
while (it != end) e2.push_back(*(it++));
VectorValues x = VectorValues::Zero(y); // x = 0
Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
VectorValues x = VectorValues::Zero(y); // x = 0
Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2
axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x
}
/* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const {
cout << s << endl;
Ab2_->print();
}
/*****************************************************************************/
void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const
{
/* copy first */
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
BOOST_REVERSE_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
/* collect a subvector of x that consists of the parents of cg (S) */
const Vector xParent = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginParents(), cg->endParents()));
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
/* compute the solution for the current pivot */
const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().solve(rhsFrontal - cg->get_S() * xParent);
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
}
}
/*****************************************************************************/
void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const
{
/* copy first */
std::copy(y.data(), y.data() + y.rows(), x.data());
/* in place back substitute */
BOOST_FOREACH(const boost::shared_ptr<GaussianConditional> & cg, *Rc1_) {
const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()));
// const Vector solFrontal = cg->get_R().triangularView<Eigen::Upper>().transpose().solve(rhsFrontal);
const Vector solFrontal = cg->get_R().transpose().triangularView<Eigen::Lower>().solve(rhsFrontal);
// Check for indeterminant solution
if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front());
/* assign subvector of sol to the frontal variables */
setSubvector(solFrontal, keyInfo_, FastVector<Key>(cg->beginFrontals(), cg->endFrontals()), x);
/* substract from parent variables */
for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) {
KeyInfo::const_iterator it2 = keyInfo_.find(*it);
Eigen::Map<Vector> rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1);
rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal;
}
}
}
/*****************************************************************************/
void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map<Key,Vector> &lambda)
{
/* identify the subgraph structure */
const SubgraphBuilder builder(parameters_.builderParams_);
Subgraph::shared_ptr subgraph = builder(gfg);
keyInfo_ = keyInfo;
/* build factor subgraph */
GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true);
/* factorize and cache BayesNet */
Rc1_ = gfg_subgraph->eliminateSequential();
}
/*****************************************************************************/
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys) {
/* a cache of starting index and dim */
typedef vector<pair<size_t, size_t> > Cache;
Cache cache;
/* figure out dimension by traversing the keys */
size_t d = 0;
BOOST_FOREACH ( const Key &key, keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
cache.push_back(make_pair(entry.colstart(), entry.dim()));
d += entry.dim();
}
/* ************************************************************************* */
void SubgraphPreconditioner::print(const std::string& s) const {
cout << s << endl;
Ab2_->print();
/* use the cache to fill the result */
Vector result = Vector::Zero(d, 1);
size_t idx = 0;
BOOST_FOREACH ( const Cache::value_type &p, cache ) {
result.segment(idx, p.second) = src.segment(p.first, p.second) ;
idx += p.second;
}
return result;
}
/*****************************************************************************/
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst) {
/* use the cache */
size_t idx = 0;
BOOST_FOREACH ( const Key &key, keys ) {
const KeyInfoEntry &entry = keyInfo.find(key)->second;
dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ;
idx += entry.dim();
}
}
/*****************************************************************************/
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) {
GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
result->reserve(subgraph.size());
BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) {
const size_t idx = e.index();
if ( clone ) result->push_back(gfg[idx]->clone());
else result->push_back(gfg[idx]);
}
return result;
}
} // nsamespace gtsam

206
gtsam/linear/SubgraphPreconditioner.h
View File

@ -12,15 +12,16 @@
/**
* @file SubgraphPreconditioner.h
* @date Dec 31, 2009
* @author Frank Dellaert
* @author Frank Dellaert, Yong-Dian Jian
*/
#pragma once
#include <gtsam/global_includes.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/Errors.h>
#include <gtsam/linear/IterativeSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/shared_ptr.hpp>
namespace gtsam {
@ -30,6 +31,144 @@ namespace gtsam {
class GaussianFactorGraph;
class VectorValues;
struct GTSAM_EXPORT SubgraphEdge {
size_t index_; /* edge id */
double weight_; /* edge weight */
SubgraphEdge() : index_(0), weight_(1.0) {}
SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {}
SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {}
inline size_t index() const { return index_; }
inline double weight() const { return weight_; }
inline bool isUnitWeight() const { return (weight_ == 1.0); }
friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(index_);
ar & BOOST_SERIALIZATION_NVP(weight_);
}
};
/**************************************************************************/
class GTSAM_EXPORT Subgraph {
public:
typedef boost::shared_ptr<Subgraph> shared_ptr;
typedef std::vector<shared_ptr> vector_shared_ptr;
typedef std::vector<SubgraphEdge> Edges;
typedef std::vector<size_t> EdgeIndices;
typedef Edges::iterator iterator;
typedef Edges::const_iterator const_iterator;
protected:
Edges edges_; /* index to the factors */
public:
Subgraph() {}
Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {}
Subgraph(const Edges &edges) : edges_(edges) {}
Subgraph(const std::vector<size_t> &indices) ;
inline const Edges& edges() const { return edges_; }
inline const size_t size() const { return edges_.size(); }
EdgeIndices edgeIndices() const;
iterator begin() { return edges_.begin(); }
const_iterator begin() const { return edges_.begin(); }
iterator end() { return edges_.end(); }
const_iterator end() const { return edges_.end(); }
void save(const std::string &fn) const;
static shared_ptr load(const std::string &fn);
friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph);
private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(edges_);
}
};
/****************************************************************************/
struct GTSAM_EXPORT SubgraphBuilderParameters {
public:
typedef boost::shared_ptr<SubgraphBuilderParameters> shared_ptr;
enum Skeleton {
/* augmented tree */
NATURALCHAIN = 0, /* natural ordering of the graph */
BFS, /* breadth-first search tree */
KRUSKAL, /* maximum weighted spanning tree */
} skeleton_ ;
enum SkeletonWeight { /* how to weigh the graph edges */
EQUAL = 0, /* every block edge has equal weight */
RHS_2NORM, /* use the 2-norm of the rhs */
LHS_FNORM, /* use the frobenius norm of the lhs */
RANDOM, /* bounded random edge weight */
} skeletonWeight_ ;
enum AugmentationWeight { /* how to weigh the graph edges */
SKELETON = 0, /* use the same weights in building the skeleton */
// STRETCH, /* stretch in the laplacian sense */
// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */
} augmentationWeight_ ;
double complexity_;
SubgraphBuilderParameters()
: skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {}
virtual ~SubgraphBuilderParameters() {}
/* for serialization */
void print() const ;
virtual void print(std::ostream &os) const ;
friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p);
static Skeleton skeletonTranslator(const std::string &s);
static std::string skeletonTranslator(Skeleton w);
static SkeletonWeight skeletonWeightTranslator(const std::string &s);
static std::string skeletonWeightTranslator(SkeletonWeight w);
static AugmentationWeight augmentationWeightTranslator(const std::string &s);
static std::string augmentationWeightTranslator(AugmentationWeight w);
};
/*****************************************************************************/
class GTSAM_EXPORT SubgraphBuilder {
public:
typedef SubgraphBuilder Base;
typedef boost::shared_ptr<SubgraphBuilder> shared_ptr;
typedef std::vector<double> Weights;
SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: parameters_(p) {}
virtual ~SubgraphBuilder() {}
virtual boost::shared_ptr<Subgraph> operator() (const GaussianFactorGraph &jfg) const ;
private:
std::vector<size_t> buildTree(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &weights) const ;
std::vector<size_t> unary(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> natural_chain(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> bfs(const GaussianFactorGraph &gfg) const ;
std::vector<size_t> kruskal(const GaussianFactorGraph &gfg, const FastMap<Key, size_t> &ordering, const std::vector<double> &w) const ;
std::vector<size_t> sample(const std::vector<double> &weights, const size_t t) const ;
Weights weights(const GaussianFactorGraph &gfg) const;
SubgraphBuilderParameters parameters_;
};
/*******************************************************************************************/
struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters {
typedef PreconditionerParameters Base;
typedef boost::shared_ptr<SubgraphPreconditionerParameters> shared_ptr;
SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters())
: Base(), builderParams_(p) {}
virtual ~SubgraphPreconditionerParameters() {}
SubgraphBuilderParameters builderParams_;
};
/**
* Subgraph conditioner class, as explained in the RSS 2010 submission.
* Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2
@ -37,7 +176,7 @@ namespace gtsam {
* To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2.
* Then solve for yhat using CG, and solve for xhat = system.x(yhat).
*/
class GTSAM_EXPORT SubgraphPreconditioner {
class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner {
public:
typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
@ -52,9 +191,12 @@ namespace gtsam {
sharedValues xbar_; ///< A1 \ b1
sharedErrors b2bar_; ///< A2*xbar - b2
KeyInfo keyInfo_;
SubgraphPreconditionerParameters parameters_;
public:
SubgraphPreconditioner();
SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
/**
* Constructor
@ -62,7 +204,10 @@ namespace gtsam {
* @param Rc1: the Bayes Net R1*x=c1
* @param xbar: the solution to R1*x=c1
*/
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar,
const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters());
virtual ~SubgraphPreconditioner() {}
/** print the object */
void print(const std::string& s = "SubgraphPreconditioner") const;
@ -80,7 +225,6 @@ namespace gtsam {
* Add zero-mean i.i.d. Gaussian prior terms to each variable
* @param sigma Standard deviation of Gaussian
*/
// SubgraphPreconditioner add_priors(double sigma) const;
/* x = xbar + inv(R1)*y */
VectorValues x(const VectorValues& y) const;
@ -119,6 +263,54 @@ namespace gtsam {
* y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2]
*/
void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const;
/*****************************************************************************/
/* implement virtual functions of Preconditioner */
/* Computation Interfaces for Vector */
virtual void solve(const Vector& y, Vector &x) const;
virtual void transposeSolve(const Vector& y, Vector& x) const ;
virtual void build(
const GaussianFactorGraph &gfg,
const KeyInfo &info,
const std::map<Key,Vector> &lambda
) ;
/*****************************************************************************/
};
/* get subvectors */
Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys);
/* set subvectors */
void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector<Key> &keys, Vector &dst);
/* build a factor subgraph, which is defined as a set of weighted edges (factors) */
boost::shared_ptr<GaussianFactorGraph>
buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone);
/* sort the container and return permutation index with default comparator */
template <typename Container>
std::vector<size_t> sort_idx(const Container &src)
{
typedef typename Container::value_type T;
const size_t n = src.size() ;
std::vector<std::pair<size_t,T> > tmp;
tmp.reserve(n);
for ( size_t i = 0 ; i < n ; i++ )
tmp.push_back(std::make_pair(i, src[i]));
/* sort */
std::stable_sort(tmp.begin(), tmp.end()) ;
/* copy back */
std::vector<size_t> idx; idx.reserve(n);
for ( size_t i = 0 ; i < n ; i++ ) {
idx.push_back(tmp[i].first) ;
}
return idx;
}
} // namespace gtsam

8
gtsam/linear/SubgraphSolver.cpp
View File

@ -14,6 +14,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/iterative-inl.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph-inl.h>
@ -143,4 +144,11 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
return boost::tie(At, Ac);
}
/****************************************************************************/
VectorValues SubgraphSolver::optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) { return VectorValues(); }
} // \namespace gtsam

22
gtsam/linear/SubgraphSolver.h
View File

@ -13,22 +13,23 @@
#include <gtsam/linear/ConjugateGradientSolver.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/inference/Ordering.h>
#include <boost/tuple/tuple.hpp>
#include <iosfwd>
namespace gtsam {
// Forward declarations
class GaussianFactorGraph;
class GaussianBayesNet;
class SubgraphPreconditioner;
class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters {
public:
typedef ConjugateGradientParameters Base;
SubgraphSolverParameters() : Base() {}
virtual void print() const { Base::print(); }
void print() const { Base::print(); }
virtual void print(std::ostream &os) const { Base::print(os); }
};
/**
@ -61,7 +62,7 @@ public:
protected:
Parameters parameters_;
Ordering ordering_;
SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object
boost::shared_ptr<SubgraphPreconditioner> pc_; ///< preconditioner object
public:
/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
@ -77,8 +78,17 @@ public:
SubgraphSolver(const boost::shared_ptr<GaussianBayesNet> &Rc1, const boost::shared_ptr<GaussianFactorGraph> &Ab2, const Parameters &parameters, const Ordering& ordering);
virtual ~SubgraphSolver() {}
virtual VectorValues optimize () ;
virtual VectorValues optimize (const VectorValues &initial) ;
VectorValues optimize () ;
VectorValues optimize (const VectorValues &initial) ;
/* interface to the nonlinear optimizer that the subclasses have to implement */
virtual VectorValues optimize (
const GaussianFactorGraph &gfg,
const KeyInfo &keyInfo,
const std::map<Key, Vector> &lambda,
const VectorValues &initial
) ;
protected:

186
gtsam/navigation/CombinedImuFactor.h
View File

@ -11,7 +11,7 @@
/**
* @file CombinedImuFactor.h
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
**/
#pragma once
@ -71,8 +71,9 @@ namespace gtsam {
Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
bool use2ndOrderIntegration_; ///< Controls the order of integration
///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases
///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega]
/** Default constructor, initialize with no IMU measurements */
@ -83,12 +84,14 @@ namespace gtsam {
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution)
const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution)
const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements
const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements
const bool use2ndOrderIntegration = false ///< Controls the order of integration
///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements)
) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15))
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)),
use2ndOrderIntegration_(use2ndOrderIntegration)
{
// COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21)
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
@ -136,6 +139,19 @@ namespace gtsam {
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
}
void resetIntegration(){
deltaPij = Vector3::Zero();
deltaVij = Vector3::Zero();
deltaRij = Rot3();
deltaTij = 0.0;
delPdelBiasAcc = Matrix3::Zero();
delPdelBiasOmega = Matrix3::Zero();
delVdelBiasAcc = Matrix3::Zero();
delVdelBiasOmega = Matrix3::Zero();
delRdelBiasOmega = Matrix3::Zero();
PreintMeasCov = Matrix::Zero(15,15);
}
/** Add a single IMU measurement to the preintegration. */
void integrateMeasurement(
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
@ -163,11 +179,14 @@ namespace gtsam {
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
// delPdelBiasAcc += delVdelBiasAcc * deltaT;
// delPdelBiasOmega += delVdelBiasOmega * deltaT;
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
* skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
if(!use2ndOrderIntegration_){
delPdelBiasAcc += delVdelBiasAcc * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT;
}else{
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
* skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
}
delVdelBiasAcc += -deltaRij.matrix() * deltaT;
delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
@ -222,32 +241,31 @@ namespace gtsam {
G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3);
G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) *
(measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) *
(H_vel_biasacc.transpose());
(measurementCovariance.block(3,3,3,3) + measurementCovariance.block(15,15,3,3) ) *
(H_vel_biasacc.transpose());
G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) *
(measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) *
(H_angles_biasomega.transpose());
(measurementCovariance.block(6,6,3,3) + measurementCovariance.block(18,18,3,3) ) *
(H_angles_biasomega.transpose());
G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3);
G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3);
// OFF BLOCK DIAGONAL TERMS
Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3);
G_measCov_Gt.block(3,9,3,3) = block24;
G_measCov_Gt.block(9,3,3,3) = block24.transpose();
Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3);
G_measCov_Gt.block(6,12,3,3) = block35;
G_measCov_Gt.block(12,6,3,3) = block35.transpose();
// NEW OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * measurementCovariance.block(18,15,3,3) * H_angles_biasomega.transpose();
G_measCov_Gt.block(3,6,3,3) = block23;
G_measCov_Gt.block(6,3,3,3) = block23.transpose();
PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt;
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
// deltaPij += deltaVij * deltaT;
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
if(!use2ndOrderIntegration_){
deltaPij += deltaVij * deltaT;
}else{
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
}
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
@ -311,23 +329,40 @@ namespace gtsam {
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
/** Shorthand for a smart pointer to a factor */
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<CombinedImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<CombinedImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {}
/** Constructor */
CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
const SharedNoiseModel& model, boost::optional<const Pose3&> body_P_sensor = boost::none) :
Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
CombinedImuFactor(
Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key
Key bias_i, ///< previous bias key
Key bias_j, ///< current bias key
const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
const Vector3& gravity, ///< gravity vector
const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor) {
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis){
}
virtual ~CombinedImuFactor() {}
@ -360,7 +395,7 @@ namespace gtsam {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
@ -370,6 +405,10 @@ namespace gtsam {
const CombinedPreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
/** implement functions needed to derive from Factor */
/** vector of errors */
@ -414,19 +453,18 @@ namespace gtsam {
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
if(H1) {
H1->resize(15,6);
/*
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
- Rot_i.matrix(),
- Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
Matrix3::Zero(),
skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
@ -435,6 +473,40 @@ namespace gtsam {
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPi
Matrix3::Zero(), Matrix3::Zero();
*/
if(H1) {
H1->resize(15,6);
Matrix3 dfPdPi;
Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
}
else{
dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero();
}
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero(),
//dBiasAcc/dPi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPi
Matrix3::Zero(), Matrix3::Zero();
}
if(H2) {
@ -445,14 +517,13 @@ namespace gtsam {
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero(),
//dBiasAcc/dVi
Matrix3::Zero(),
//dBiasOmega/dVi
Matrix3::Zero();
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero(),
//dBiasAcc/dVi
Matrix3::Zero(),
//dBiasOmega/dVi
Matrix3::Zero();
}
if(H3) {
@ -524,7 +595,6 @@ namespace gtsam {
Matrix3::Zero(), Matrix3::Identity();
}
// Evaluate residual error, according to [3]
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 fp =
@ -559,7 +629,8 @@ namespace gtsam {
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
@ -571,18 +642,23 @@ namespace gtsam {
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)

162
gtsam/navigation/ImuFactor.h
View File

@ -11,7 +11,7 @@
/**
* @file ImuFactor.h
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman
* @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen
**/
#pragma once
@ -60,7 +60,7 @@ namespace gtsam {
imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration
Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i)
Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i)
Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame)
Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i)
double deltaTij; ///< Time interval from i to j
@ -70,19 +70,20 @@ namespace gtsam {
Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias
Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias
Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*)
bool use2ndOrderIntegration_; ///< Controls the order of integration
/** Default constructor, initialize with no IMU measurements */
PreintegratedMeasurements(
const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases
const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc
const Matrix3& integrationErrorCovariance ///< Covariance matrix of measuredAcc
const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc
const bool use2ndOrderIntegration = false ///< Controls the order of integration
) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0),
delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()),
delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()),
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9)
delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(use2ndOrderIntegration)
{
measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(),
Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(),
@ -127,6 +128,19 @@ namespace gtsam {
&& equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol);
}
void resetIntegration(){
deltaPij = Vector3::Zero();
deltaVij = Vector3::Zero();
deltaRij = Rot3();
deltaTij = 0.0;
delPdelBiasAcc = Matrix3::Zero();
delPdelBiasOmega = Matrix3::Zero();
delVdelBiasAcc = Matrix3::Zero();
delVdelBiasOmega = Matrix3::Zero();
delRdelBiasOmega = Matrix3::Zero();
PreintMeasCov = Matrix::Zero(9,9);
}
/** Add a single IMU measurement to the preintegration. */
void integrateMeasurement(
const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame)
@ -143,11 +157,8 @@ namespace gtsam {
// Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame
if(body_P_sensor){
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega);
correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector();
// linear acceleration vector in the body frame
}
@ -159,16 +170,19 @@ namespace gtsam {
// Update Jacobians
/* ----------------------------------------------------------------------------------------------------------------------- */
// delPdelBiasAcc += delVdelBiasAcc * deltaT;
// delPdelBiasOmega += delVdelBiasOmega * deltaT;
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
* skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
if(!use2ndOrderIntegration_){
delPdelBiasAcc += delVdelBiasAcc * deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT;
}else{
delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT;
delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix()
* skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega;
}
delVdelBiasAcc += -deltaRij.matrix() * deltaT;
delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega;
delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT;
// Update preintegrated mesurements covariance
// Update preintegrated measurements covariance
/* ----------------------------------------------------------------------------------------------------------------------- */
Matrix3 Z_3x3 = Matrix3::Zero();
Matrix3 I_3x3 = Matrix3::Identity();
@ -210,7 +224,11 @@ namespace gtsam {
// Update preintegrated measurements
/* ----------------------------------------------------------------------------------------------------------------------- */
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
if(!use2ndOrderIntegration_){
deltaPij += deltaVij * deltaT;
}else{
deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
}
deltaVij += deltaRij.matrix() * correctedAcc * deltaT;
deltaRij = deltaRij * Rincr;
deltaTij += deltaT;
@ -274,24 +292,39 @@ namespace gtsam {
Vector3 omegaCoriolis_;
boost::optional<Pose3> body_P_sensor_; ///< The pose of the sensor in the body frame
bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect
public:
/** Shorthand for a smart pointer to a factor */
#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5
typedef typename boost::shared_ptr<ImuFactor> shared_ptr;
#else
typedef boost::shared_ptr<ImuFactor> shared_ptr;
#endif
/** Default constructor - only use for serialization */
ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {}
/** Constructor */
ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis,
boost::optional<const Pose3&> body_P_sensor = boost::none) :
ImuFactor(
Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key
Key bias, ///< previous bias key
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
const Vector3& gravity, ///< gravity vector
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
boost::optional<const Pose3&> body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame
const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect
) :
Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias),
preintegratedMeasurements_(preintegratedMeasurements),
gravity_(gravity),
omegaCoriolis_(omegaCoriolis),
body_P_sensor_(body_P_sensor) {
body_P_sensor_(body_P_sensor),
use2ndOrderCoriolis_(use2ndOrderCoriolis){
}
virtual ~ImuFactor() {}
@ -323,7 +356,7 @@ namespace gtsam {
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_)
&& preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol)
&& equal_with_abs_tol(gravity_, e->gravity_, tol)
&& equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol)
&& ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_)));
@ -333,6 +366,10 @@ namespace gtsam {
const PreintegratedMeasurements& preintegratedMeasurements() const {
return preintegratedMeasurements_; }
const Vector3& gravity() const { return gravity_; }
const Vector3& omegaCoriolis() const { return omegaCoriolis_; }
/** implement functions needed to derive from Factor */
/** vector of errors */
@ -378,22 +415,35 @@ namespace gtsam {
if(H1) {
H1->resize(9,6);
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
- Rot_i.matrix(),
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
Matrix3::Zero(),
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero();
Matrix3 dfPdPi;
Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
}
else{
dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero();
}
(*H1) <<
// dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi
dfPdPi,
// dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi
dfVdPi,
// dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi
Matrix3::Zero();
}
if(H2) {
H2->resize(9,3);
(*H2) <<
@ -402,11 +452,11 @@ namespace gtsam {
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi
- Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero();
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi
Matrix3::Zero();
}
if(H3) {
H3->resize(9,6);
@ -418,6 +468,7 @@ namespace gtsam {
// dfR/dPosej
Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero();
}
if(H4) {
H4->resize(9,3);
(*H4) <<
@ -428,6 +479,7 @@ namespace gtsam {
// dfR/dVj
Matrix3::Zero();
}
if(H5) {
const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected);
@ -475,7 +527,8 @@ namespace gtsam {
/** predicted states from IMU */
static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none)
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false)
{
const double& deltaTij = preintegratedMeasurements.deltaTij;
@ -487,18 +540,23 @@ namespace gtsam {
// Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */
const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij);
if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
}
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
// deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr)

4
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -183,7 +183,7 @@ TEST( CombinedImuFactor, ErrorWithBiases )
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel);
CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis);
Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias);
@ -260,7 +260,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
#include <gtsam/linear/GaussianFactorGraph.h>

14
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -136,8 +136,9 @@ TEST( ImuFactor, PreintegratedMeasurements )
Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0);
double expectedDeltaT1(0.5);
bool use2ndOrderIntegration = true;
// Actual preintegrated values
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6));
@ -177,7 +178,8 @@ TEST( ImuFactor, Error )
Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0;
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector();
double deltaT = 1.0;
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
bool use2ndOrderIntegration = true;
ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration);
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
// Create factor
@ -217,7 +219,7 @@ TEST( ImuFactor, Error )
Matrix H1atop6 = H1a.topRows(6);
EXPECT(assert_equal(H1etop6, H1atop6));
// rotations
EXPECT(assert_equal(RH1e, H1a.bottomRows(3))); // evaluate only the rotation residue
EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H2e, H2a));
@ -226,7 +228,7 @@ TEST( ImuFactor, Error )
Matrix H3atop6 = H3a.topRows(6);
EXPECT(assert_equal(H3etop6, H3atop6));
// rotations
EXPECT(assert_equal(RH3e, H3a.bottomRows(3))); // evaluate only the rotation residue
EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations
EXPECT(assert_equal(H4e, H4a));
// EXPECT(assert_equal(H5e, H5a));
@ -324,7 +326,7 @@ TEST( ImuFactor, PartialDerivativeExpmap )
Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign
// Compare Jacobians
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega));
EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
@ -436,7 +438,7 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc));
EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega));
EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3)));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega));
EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations
}
//#include <gtsam/linear/GaussianFactorGraph.h>

2
gtsam/nonlinear/DoglegOptimizer.cpp
View File

@ -76,7 +76,7 @@ void DoglegOptimizer::iterate(void) {
result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION,
dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose);
}
else if ( params_.isCG() ) {
else if ( params_.isIterative() ) {
throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver");
}
else {

1
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -19,6 +19,7 @@
#pragma once
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/VectorValues.h>
#include <boost/date_time/posix_time/posix_time.hpp>
class NonlinearOptimizerMoreOptimizationTest;

29
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -21,6 +21,7 @@
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
@ -106,23 +107,21 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg,
delta = gfg.optimize(*params.ordering, params.getEliminationFunction());
} else if (params.isSequential()) {
// Sequential QR or Cholesky (decided by params.getEliminationFunction())
delta = gfg.eliminateSequential(*params.ordering,
params.getEliminationFunction())->optimize();
} else if (params.isCG()) {
delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize();
} else if (params.isIterative()) {
// Conjugate Gradient -> needs params.iterativeParams
if (!params.iterativeParams)
throw std::runtime_error(
"NonlinearOptimizer::solve: cg parameter has to be assigned ...");
// the type of params.iterativeParams decides the type of CG solver
if (boost::dynamic_pointer_cast<SubgraphSolverParameters>(
params.iterativeParams)) {
SubgraphSolver solver(gfg,
dynamic_cast<const SubgraphSolverParameters&>(*params.iterativeParams),
*params.ordering);
delta = solver.optimize();
} else {
throw std::runtime_error(
"NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ...");
throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ...");
if (boost::shared_ptr<PCGSolverParameters> pcg = boost::dynamic_pointer_cast<PCGSolverParameters>(params.iterativeParams) ) {
delta = PCGSolver(*pcg).optimize(gfg);
}
else if (boost::shared_ptr<SubgraphSolverParameters> spcg = boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize();
}
else {
throw std::runtime_error("NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ...");
}
} else {
throw std::runtime_error(

16
gtsam/nonlinear/NonlinearOptimizerParams.cpp
View File

@ -66,8 +66,8 @@ std::string NonlinearOptimizerParams::verbosityTranslator(
/* ************************************************************************* */
void NonlinearOptimizerParams::setIterativeParams(
const SubgraphSolverParameters &params) {
iterativeParams = boost::make_shared<SubgraphSolverParameters>(params);
const boost::shared_ptr<IterativeOptimizationParameters> params) {
iterativeParams = params;
}
/* ************************************************************************* */
@ -99,8 +99,8 @@ void NonlinearOptimizerParams::print(const std::string& str) const {
case CHOLMOD:
std::cout << " linear solver type: CHOLMOD\n";
break;
case CONJUGATE_GRADIENT:
std::cout << " linear solver type: CONJUGATE GRADIENT\n";
case Iterative:
std::cout << " linear solver type: ITERATIVE\n";
break;
default:
std::cout << " linear solver type: (invalid)\n";
@ -127,8 +127,8 @@ std::string NonlinearOptimizerParams::linearSolverTranslator(
return "SEQUENTIAL_CHOLESKY";
case SEQUENTIAL_QR:
return "SEQUENTIAL_QR";
case CONJUGATE_GRADIENT:
return "CONJUGATE_GRADIENT";
case Iterative:
return "ITERATIVE";
case CHOLMOD:
return "CHOLMOD";
default:
@ -148,8 +148,8 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve
return SEQUENTIAL_CHOLESKY;
if (linearSolverType == "SEQUENTIAL_QR")
return SEQUENTIAL_QR;
if (linearSolverType == "CONJUGATE_GRADIENT")
return CONJUGATE_GRADIENT;
if (linearSolverType == "ITERATIVE")
return Iterative;
if (linearSolverType == "CHOLMOD")
return CHOLMOD;
throw std::invalid_argument(

10
gtsam/nonlinear/NonlinearOptimizerParams.h
View File

@ -99,7 +99,7 @@ public:
MULTIFRONTAL_QR,
SEQUENTIAL_CHOLESKY,
SEQUENTIAL_QR,
CONJUGATE_GRADIENT, /* Experimental Flag */
Iterative, /* Experimental Flag */
CHOLMOD, /* Experimental Flag */
};
@ -121,8 +121,8 @@ public:
return (linearSolverType == CHOLMOD);
}
inline bool isCG() const {
return (linearSolverType == CONJUGATE_GRADIENT);
inline bool isIterative() const {
return (linearSolverType == Iterative);
}
GaussianFactorGraph::Eliminate getEliminationFunction() const {
@ -148,7 +148,9 @@ public:
void setLinearSolverType(const std::string& solver) {
linearSolverType = linearSolverTranslator(solver);
}
void setIterativeParams(const SubgraphSolverParameters& params);
void setIterativeParams(const boost::shared_ptr<IterativeOptimizationParameters> params);
void setOrdering(const Ordering& ordering) {
this->ordering = ordering;
}

10
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -288,7 +288,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3),1e-8));
if(isDebugTest) tictoc_print_();
}
@ -365,7 +365,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
// result.print("results of 3 camera, 3 landmark optimization \n");
if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-7));
if(isDebugTest) tictoc_print_();
}
@ -428,7 +428,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-8));
}
/* *************************************************************************/
@ -631,7 +631,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-8));
}
/* *************************************************************************/
@ -697,7 +697,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){
Values result = optimizer.optimize();
if(isDebugTest) result.at<Pose3>(x3).print("Pose3 after optimization: ");
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
EXPECT(assert_equal(pose3,result.at<Pose3>(x3), 1e-7));
}
/* *************************************************************************/

5
gtsam_unstable/CMakeLists.txt
View File

@ -28,7 +28,7 @@ set (excluded_headers # "")
"${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h"
)
# assemble core libaries
# assemble core libraries
foreach(subdir ${gtsam_unstable_subdirs})
# Build convenience libraries
file(GLOB subdir_srcs "${subdir}/*.cpp")
@ -63,7 +63,7 @@ set(gtsam_unstable_soversion ${GTSAM_VERSION_MAJOR})
message(STATUS "GTSAM_UNSTABLE Version: ${gtsam_unstable_version}")
message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}")
# build shared and static versions of the library
# build shared or static versions of the library
if (GTSAM_BUILD_STATIC_LIBRARY)
message(STATUS "Building GTSAM_UNSTABLE - static")
add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs})
@ -116,7 +116,6 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}")
endif(GTSAM_INSTALL_MATLAB_TOOLBOX)
# Build examples
add_subdirectory(examples)

6
gtsam_unstable/examples/SmartProjectionFactorExample.cpp
View File

@ -108,9 +108,13 @@ int main(int argc, char** argv){
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;

2
gtsam_unstable/gtsam_unstable.h
View File

@ -23,6 +23,7 @@ virtual class gtsam::NoiseModelFactor4;
virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor;
virtual class gtsam::Cal3_S2;
class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph;
class gtsam::Ordering;
@ -379,7 +380,6 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/RangeFactor.h>
template<POSE, POINT>
virtual class RangeFactor : gtsam::NonlinearFactor {

7
matlab/unstable_examples/+imuSimulator/+lib/antisim.m Normal file
View File

@ -0,0 +1,7 @@
function S = antisim(v)
% costruisce la matrice antisimmetrica S (3x3) a partire dal vettore v
% Uso: S = antisim(v)
S=[0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0];

140
matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m Normal file
View File

@ -0,0 +1,140 @@
function arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio, rhoRatio)
% arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Used to plot a single 3D arrow with a cylindrical stem and cone arrowhead
% pos = [X,Y,Z] - spatial location of the starting point of the arrow (end of stem)
% deltaValues = [QX,QY,QZ] - delta parameters denoting the magnitude of the arrow along the x,y,z-axes (relative to 'pos')
% colorCode - Color parameters as per the 'surf' command. For example, 'r', 'red', [1 0 0] are all examples of a red-colored arrow
% stemRatio - The ratio of the length of the stem in proportion to the arrowhead. For example, a call of:
% arrow3D([0,0,0], [100,0,0] , 'r', 0.82) will produce a red arrow of magnitude 100, with the arrowstem spanning a distance
% of 82 (note 0.82 ratio of length 100) while the arrowhead (cone) spans 18.
% rhoRatio - The ratio of the cylinder radius (0.05 is the default)
% value)
%
% Example:
% arrow3D([0,0,0], [4,3,7]); %---- arrow with default parameters
% axis equal;
%
% Author: Shawn Arseneau
% Created: September 14, 2006
% Updated: September 18, 2006
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
if nargin<2 || nargin>5
error('Incorrect number of inputs to arrow3D');
end
if numel(pos)~=3 || numel(deltaValues)~=3
error('pos and/or deltaValues is incorrect dimensions (should be three)');
end
if nargin<3
colorCode = 'interp';
end
if nargin<4
stemRatio = 0.75;
end
if nargin<5
rhoRatio = 0.05;
end
X = pos(1); %---- with this notation, there is no need to transpose if the user has chosen a row vs col vector
Y = pos(2);
Z = pos(3);
[sphi, stheta, srho] = cart2sph(deltaValues(1), deltaValues(2), deltaValues(3));
%******************************************* CYLINDER == STEM *********************************************
cylinderRadius = srho*rhoRatio;
cylinderLength = srho*stemRatio;
[CX,CY,CZ] = cylinder(cylinderRadius);
CZ = CZ.*cylinderLength; %---- lengthen
%----- ROTATE CYLINDER
[row, col] = size(CX); %---- initial rotation to coincide with X-axis
newEll = rotatePoints([0 0 -1], [CX(:), CY(:), CZ(:)]);
CX = reshape(newEll(:,1), row, col);
CY = reshape(newEll(:,2), row, col);
CZ = reshape(newEll(:,3), row, col);
[row, col] = size(CX);
newEll = rotatePoints(deltaValues, [CX(:), CY(:), CZ(:)]);
stemX = reshape(newEll(:,1), row, col);
stemY = reshape(newEll(:,2), row, col);
stemZ = reshape(newEll(:,3), row, col);
%----- TRANSLATE CYLINDER
stemX = stemX + X;
stemY = stemY + Y;
stemZ = stemZ + Z;
%******************************************* CONE == ARROWHEAD *********************************************
coneLength = srho*(1-stemRatio);
coneRadius = cylinderRadius*1.5;
incr = 4; %---- Steps of cone increments
coneincr = coneRadius/incr;
[coneX, coneY, coneZ] = cylinder(cylinderRadius*2:-coneincr:0); %---------- CONE
coneZ = coneZ.*coneLength;
%----- ROTATE CONE
[row, col] = size(coneX);
newEll = rotatePoints([0 0 -1], [coneX(:), coneY(:), coneZ(:)]);
coneX = reshape(newEll(:,1), row, col);
coneY = reshape(newEll(:,2), row, col);
coneZ = reshape(newEll(:,3), row, col);
newEll = rotatePoints(deltaValues, [coneX(:), coneY(:), coneZ(:)]);
headX = reshape(newEll(:,1), row, col);
headY = reshape(newEll(:,2), row, col);
headZ = reshape(newEll(:,3), row, col);
%---- TRANSLATE CONE
V = [0, 0, srho*stemRatio]; %---- centerline for cylinder: the multiplier is to set the cone 'on the rim' of the cylinder
Vp = rotatePoints([0 0 -1], V);
Vp = rotatePoints(deltaValues, Vp);
headX = headX + Vp(1) + X;
headY = headY + Vp(2) + Y;
headZ = headZ + Vp(3) + Z;
%************************************************************************************************************
hStem = surf(stemX, stemY, stemZ, 'FaceColor', colorCode, 'EdgeColor', 'none');
hold on;
hHead = surf(headX, headY, headZ, 'FaceColor', colorCode, 'EdgeColor', 'none');
if nargout==1
arrowHandle = [hStem, hHead];
end

14
matlab/unstable_examples/+imuSimulator/+lib/getxyz.m Normal file
View File

@ -0,0 +1,14 @@
function [c] = getxyz(poses, j)
% The function extract the Cartesian variables from pose (pose.p = positions,
% pose.R = rotations). In particular, if there are T poses,
% - getxyz(pose, 1) estracts the vector x \in R^T,
% - getxyz(pose, 2) estracts the vector y \in R^T,
% - getxyz(pose, 3) estracts the vector z \in R^T.
L = length(poses);
c = [];
for i=1:L % for each pose
c = [c poses(i).p(j)];
end
c = c(:); % column vector

19
matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m Normal file
View File

@ -0,0 +1,19 @@
function [] = plot_trajectory(pose, step, color, plot_name,a,b,c)
% Plot a collection of poses: positions are connected by a solid line
% of color "color" and it is shown a ref frame for each pose.
% Plot_name defines the name of the created figure.
x = getxyz(pose,1);
y = getxyz(pose,2);
z = getxyz(pose,3);
plot3(x,y,z,color)
n = length(x)-1;
hold on
for i=1:step:n+1
ref_frame_plot(pose(i).R,pose(i).p,a,b,c)
end
title(plot_name);
xlabel('x')
ylabel('y')
zlabel('z')
view(3)

54
matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m Normal file
View File

@ -0,0 +1,54 @@
function ref_frame_plot(Ref,OriginRef,rhoRatio,stemRatio,axsize)
% ref_frame plot a 3D representation of a reference frame
% given by three unit vectors
%
% ref_frame(Ref,DimSpace,OriginRef)
% Ref is a 3 x 3 orthogonal matrix representing the unit vectors
% of the reference frame to be drawn
% DimSpace is a 3 x 2 matrix with min an max dimensions of the space
% [xmin xmax; ymin ymax; zmin zmax]
% default value = [-1,5 +1.5] for all dimensions
% OriginRef is the reference frame origin point
% default value = [0 0 0]'
% Copright (C) Basilio Bona 2007
n=nargin;
if n == 1
OriginRef=[0 0 0]';
colorcode=['r','g','b'];
rhoRatio=0.05;
stemRatio=0.75;
axsize=1;
end
if n == 2
colorcode=['r','g','b'];
rhoRatio=0.05;
stemRatio=0.75;
axsize=1;
end
if n == 3
colorcode=['r','g','b'];
stemRatio=0.75;
axsize=1;
end
if n == 4
colorcode=['r','g','b'];
axsize=1;
end
if n == 5
colorcode=['r','g','b'];
end
% xproj=DimSpace(1,2); yproj=DimSpace(2,2); zproj=DimSpace(3,1);
%hold off;
arrow3d(OriginRef, axsize*Ref(:,1), colorcode(1), stemRatio, rhoRatio);
arrow3d(OriginRef, axsize*Ref(:,2), colorcode(2), stemRatio, rhoRatio);
arrow3d(OriginRef, axsize*Ref(:,3), colorcode(3), stemRatio, rhoRatio);
axis equal; xlabel('X'); ylabel('Y'); zlabel('Z');
% lighting phong;
% camlight left;

82
matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m Normal file
View File

@ -0,0 +1,82 @@
function rotatedData = rotatePoints(alignmentVector, originalData)
% rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%
% Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector
%
% Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1]
%
% Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho); alignmentVector = [dX,dY,dZ];
%
% Example:
% %% Rotate the point [3,4,-7] with respect to the following:
% %%%% Original associated vector is always [1,0,0]
% %%%% Calculate the appropriate rotation requested with respect to the x-axis. For example, if only a rotation about the z-axis is
% %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero
% rotData = rotatePoints(alignmentVector, [3,4,-7]);
%
% Author: Shawn Arseneau
% Created: Feb.2, 2006
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
alignmentDim = numel(alignmentVector);
DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data)
if alignmentDim~=DOF
error('Alignment vector does not agree with originalData dimensions');
end
if DOF<2 || DOF>3
error('rotatePoints only does rotation in two or three dimensions');
end
if DOF==2 % 2D rotation...
[rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2));
deg_theta = -1 * rad_theta * (180/pi);
ctheta = cosd(deg_theta); stheta = sind(deg_theta);
Rmatrix = [ctheta, -1.*stheta;...
stheta, ctheta];
rotatedData = originalData*Rmatrix;
else % 3D rotation...
[rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3));
rad_theta = rad_theta * -1;
deg_theta = rad_theta * (180/pi);
deg_phi = rad_phi * (180/pi);
ctheta = cosd(deg_theta); stheta = sind(deg_theta);
Rz = [ctheta, -1.*stheta, 0;...
stheta, ctheta, 0;...
0, 0, 1]; %% First rotate as per theta around the Z axis
rotatedData = originalData*Rz;
[rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1); %% Second rotation corresponding to phi
rotationAxis = [rotX, rotY, rotZ];
u = rotationAxis(:)/norm(rotationAxis); %% Code extract from rotate.m from MATLAB
cosPhi = cosd(deg_phi);
sinPhi = sind(deg_phi);
invCosPhi = 1 - cosPhi;
x = u(1);
y = u(2);
z = u(3);
Rmatrix = [cosPhi+x^2*invCosPhi x*y*invCosPhi-z*sinPhi x*z*invCosPhi+y*sinPhi; ...
x*y*invCosPhi+z*sinPhi cosPhi+y^2*invCosPhi y*z*invCosPhi-x*sinPhi; ...
x*z*invCosPhi-y*sinPhi y*z*invCosPhi+x*sinPhi cosPhi+z^2*invCosPhi]';
rotatedData = rotatedData*Rmatrix;
end

14
matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m Normal file
View File

@ -0,0 +1,14 @@
function R = uth2rot(u,teta)
% Uso: R = uth2rot(u,teta)
%
% calcola la matrice R
% a partire da asse u ed angolo di rotazione teta (in rad)
S=antisim(u);
t=teta;
n=norm(u);
R = eye(3) + sin(t)/n*S + (1-cos(t))/n^2*S^2;

146
matlab/unstable_examples/+imuSimulator/IMUComparison.m Normal file
View File

@ -0,0 +1,146 @@
import gtsam.*;
deltaT = 0.001;
summarizedDeltaT = 0.1;
timeElapsed = 1;
times = 0:deltaT:timeElapsed;
omega = [0;0;2*pi];
velocity = [1;0;0];
summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ...
gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ...
1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3));
%% Set initial conditions for the true trajectory and for the estimates
% (one estimate is obtained by integrating in the body frame, the other
% by integrating in the navigation frame)
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Initial state estimate (integrating in navigation frame)
currentPoseGlobalIMUnav = currentPoseGlobal;
currentVelocityGlobalIMUnav = currentVelocityGlobal;
% Initial state estimate (integrating in the body frame)
currentPoseGlobalIMUbody = currentPoseGlobal;
currentVelocityGlobalIMUbody = currentVelocityGlobal;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Trajectory estimate (integrated in the navigation frame)
positionsIMUnav = zeros(3, length(times)+1);
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUnav(1).p = positionsIMUnav(:,1);
posesIMUnav(1).R = poses(1).R;
% Trajectory estimate (integrated in the body frame)
positionsIMUbody = zeros(3, length(times)+1);
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUbody(1).p = positionsIMUbody(:,1);
posesIMUbody(1).R = poses(1).R;
%% Solver object
isamParams = ISAM2Params;
isamParams.setRelinearizeSkip(1);
isam = gtsam.ISAM2(isamParams);
initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph;
initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0)));
initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0)));
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0)));
%% Main loop
i = 2;
lastSummaryTime = 0;
lastSummaryIndex = 0;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
for t = times
%% Create the actual trajectory, using the velocities and
% accelerations in the inertial frame to compute the positions
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
%% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
%% Accumulate preintegrated measurement
currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT);
%% Update solver
if t - lastSummaryTime >= summarizedDeltaT
% Create IMU factor
initialFactors.add(ImuFactor( ...
symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ...
symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ...
symbol('b',0), currentSummarizedMeasurement, [0;0;1], [0;0;0], ...
noiseModel.Isotropic.Sigma(9, 1e-6)));
% Predict movement in a straight line (bad initialization)
if lastSummaryIndex > 0
initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ...
.compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) ));
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else
initialPose = Pose3;
initialVel = LieVector(velocity);
end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);
% Update solver
isam.update(initialFactors, initialValues);
initialFactors = NonlinearFactorGraph;
initialValues = Values;
lastSummaryIndex = lastSummaryIndex + 1;
lastSummaryTime = t;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
end
%% Integrate in the body frame
[ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ...
currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity);
%% Integrate in the navigation frame
[ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ...
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
% -
poses(i).p = positions(:,i);
posesIMUbody(i).p = positionsIMUbody(:,i);
posesIMUnav(i).p = positionsIMUnav(:,i);
% -
poses(i).R = currentPoseGlobal.rotation.matrix;
posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix;
posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix;
i = i + 1;
end
figure(1)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r');
plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k');
plot3DTrajectory(isam.calculateEstimate, 'g-');
axis equal;
legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')

128
matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m Normal file
View File

@ -0,0 +1,128 @@
close all
clc
import gtsam.*;
deltaT = 0.001;
summarizedDeltaT = 0.1;
timeElapsed = 1;
times = 0:deltaT:timeElapsed;
omega = [0;0;2*pi];
velocity = [1;0;0];
g = [0;0;0];
cor_v = [0;0;0];
summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ...
gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ...
1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3));
%% Set initial conditions for the true trajectory and for the estimates
% (one estimate is obtained by integrating in the body frame, the other
% by integrating in the navigation frame)
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
%% Solver object
isamParams = ISAM2Params;
isamParams.setRelinearizeSkip(1);
isam = gtsam.ISAM2(isamParams);
sigma_init_x = 1.0;
sigma_init_v = 1.0;
sigma_init_b = 1.0;
initialValues = Values;
initialValues.insert(symbol('x',0), currentPoseGlobal);
initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal));
initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0]));
initialFactors = NonlinearFactorGraph;
% Prior on initial pose
initialFactors.add(PriorFactorPose3(symbol('x',0), ...
currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x)));
% Prior on initial velocity
initialFactors.add(PriorFactorLieVector(symbol('v',0), ...
LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v)));
% Prior on initial bias
initialFactors.add(PriorFactorConstantBias(symbol('b',0), ...
imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b)));
%% Main loop
i = 2;
lastSummaryTime = 0;
lastSummaryIndex = 0;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
for t = times
%% Create the ground truth trajectory, using the velocities and accelerations in the inertial frame to compute the positions
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
%% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
%% Accumulate preintegrated measurement
currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT);
%% Update solver
if t - lastSummaryTime >= summarizedDeltaT
% Create IMU factor
initialFactors.add(ImuFactor( ...
symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ...
symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ...
symbol('b',0), currentSummarizedMeasurement, g, cor_v, ...
noiseModel.Isotropic.Sigma(9, 1e-6)));
% Predict movement in a straight line (bad initialization)
if lastSummaryIndex > 0
initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ...
.compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) ));
initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex));
else
initialPose = Pose3;
initialVel = LieVector(velocity);
end
initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose);
initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel);
key_pose = symbol('x',lastSummaryIndex+1);
% Update solver
isam.update(initialFactors, initialValues);
initialFactors = NonlinearFactorGraph;
initialValues = Values;
isam.calculateEstimate(key_pose);
M = isam.marginalCovariance(key_pose);
lastSummaryIndex = lastSummaryIndex + 1;
lastSummaryTime = t;
currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate);
end
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
i = i + 1;
end
figure(1)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot3DTrajectory(isam.calculateEstimate, 'g-');
axis equal;
legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')

26
matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m Normal file
View File

@ -0,0 +1,26 @@
function pos_ECEF = LatLonHRad_to_ECEF(LatLonH)
% convert latitude, longitude, height to XYZ in ECEF coordinates
% LatLonH(1) : latitude in radian
% LatLonH(2) : longitude in radian
% LatLonH(3) : height in meter
%
% Source: A. Chatfield, "Fundamentals of High Accuracy Inertial
% Navigation", 1997. pp. 10 Eq 1.18
%
% constants
a = 6378137.0; %m
e_sqr =0.006694379990141317;
% b = 6356752.3142; % m
%RAD_PER_DEG = pi/180;
phi = LatLonH(1);%*RAD_PER_DEG;
lambda = LatLonH(2);%*RAD_PER_DEG;
h = LatLonH(3);
RN = a/sqrt(1 - e_sqr*sin(phi)^2);
pos_ECEF = zeros(3,1);
pos_ECEF(1) = (RN + h )*cos(phi)*cos(lambda);
pos_ECEF(2) = (RN + h )*cos(phi)*sin(lambda);
pos_ECEF(3) = (RN*(1-e_sqr) + h)*sin(phi) ;

68
matlab/unstable_examples/+imuSimulator/OdometryExample3D.m Normal file
View File

@ -0,0 +1,68 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
% Atlanta, Georgia 30332-0415
% All Rights Reserved
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
%
% See LICENSE for the license information
%
% @brief Example of a simple 2D localization example
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Copied Original file. Modified by David Jensen to use Pose3 instead of
% Pose2. Everything else is the same.
import gtsam.*
%% Assumptions
% - Robot poses are facing along the X axis (horizontal, to the right in 2D)
% - The robot moves 2 meters each step
% - The robot is on a grid, moving 2 meters each step
%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph)
graph = NonlinearFactorGraph;
%% Add a Gaussian prior on pose x_1
priorMean = Pose3();%Pose3.Expmap([0.0; 0.0; 0.0; 0.0; 0.0; 0.0]); % prior mean is at origin
priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.3; 0.1; 0.1; 0.1]); % 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactorPose3(1, priorMean, priorNoise)); % add directly to graph
%% Add two odometry factors
odometry = Pose3.Expmap([0.0; 0.0; 0.0; 2.0; 0.0; 0.0]); % create a measurement for both factors (the same in this case)
odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.2; 0.1; 0.1; 0.1]); % 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactorPose3(1, 2, odometry, odometryNoise));
graph.add(BetweenFactorPose3(2, 3, odometry, odometryNoise));
%% print
graph.print(sprintf('\nFactor graph:\n'));
%% Initialize to noisy points
initialEstimate = Values;
%initialEstimate.insert(1, Pose3.Expmap([0.2; 0.1; 0.1; 0.5; 0.0; 0.0]));
%initialEstimate.insert(2, Pose3.Expmap([-0.2; 0.1; -0.1; 2.3; 0.1; 0.1]));
%initialEstimate.insert(3, Pose3.Expmap([0.1; -0.1; 0.1; 4.1; 0.1; -0.1]));
%initialEstimate.print(sprintf('\nInitial estimate:\n '));
for i=1:3
deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5
deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad)
deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]);
currentPose = currentPose.compose(deltaPose);
initialEstimate.insert(i, currentPose);
end
%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate);
result = optimizer.optimizeSafely();
result.print(sprintf('\nFinal result:\n '));
%% Plot trajectory and covariance ellipses
cla;
hold on;
plot3DTrajectory(result, [], Marginals(graph, result));
axis([-0.6 4.8 -1 1])
axis equal
view(2)

14
matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m Normal file
View File

@ -0,0 +1,14 @@
function [ acc_omega ] = calculateIMUMeas_coriolis(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT)
import gtsam.*;
% gyro measured rotation rate
measuredOmega = omega1Body;
% Acceleration measurement (in this simple toy example no other forces
% act on the body and the only acceleration is the centripetal Coriolis acceleration)
measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector;
acc_omega = [ measuredAcc; measuredOmega ];
end

26
matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m Normal file
View File

@ -0,0 +1,26 @@
function [ acc_omega ] = calculateIMUMeasurement(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT, imuFrame)
%CALCULATEIMUMEASUREMENT Calculate measured body frame acceleration in
%frame 1 and measured angular rates in frame 1.
import gtsam.*;
% Calculate gyro measured rotation rate by transforming body rotation rate
% into the IMU frame.
measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector;
% Transform both velocities into IMU frame, accounting for the velocity
% induced by rigid body rotation on a lever arm (Coriolis effect).
velocity1inertial = imuFrame.rotation.unrotate( ...
Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector;
imu2in1 = Rot3.Expmap(measuredOmega * deltaT);
velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ...
Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector;
% Acceleration in IMU frame
measuredAcc = (velocity2inertial - velocity1inertial) / deltaT;
acc_omega = [ measuredAcc; measuredOmega ];
end

508
matlab/unstable_examples/+imuSimulator/coriolisExample.m Normal file
View File

@ -0,0 +1,508 @@
%% coriolisExample.m
% Author(s): David Jensen (david.jensen@gtri.gatech.edu)
% This script demonstrates the relationship between object motion in inertial and rotating reference frames.
% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially
% coincident with the rotating ECEF reference frame (X towards 0 longitude, Y towards 90 longitude, Z towards 90 latitude),
% which rotates with the earth.
% A body is moving in the positive Z-direction and positive Y-direction with respect to the fixed reference frame.
% Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would
% experience the body's motion.
import gtsam.*;
addpath(genpath('./Libraries'))
% Check for an external configuarion. This is useful for setting up
% multiple tests
if ~exist('externalCoriolisConfiguration', 'var')
clc
clear all
close all
%% General configuration
navFrameRotating = 1; % 0 = perform navigation in the fixed frame
% 1 = perform navigation in the rotating frame
IMU_type = 1; % IMU type 1 or type 2
useRealisticValues = 1; % use reaslist values for initial position and earth rotation
record_movie = 0; % 0 = do not record movie
% 1 = record movie of the trajectories. One
% frame per time step (15 fps)
incrementalPlotting = 1; % turn incremental plotting on and off. Turning plotting off increases
% speed for batch testing
estimationEnabled = 1;
%% Scenario Configuration
deltaT = 0.01; % timestep
timeElapsed = 5; % Total elapsed time
times = 0:deltaT:timeElapsed;
% Initial Conditions
omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s)
radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km
if useRealisticValues == 1
omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [-0.5;0.5;2]; % constant acceleration measurement
g = [0;0;0]; % Gravity
% initial position at some [longitude, latitude] location on earth's
% surface (approximating Earth as a sphere)
initialLongitude = 45; % longitude in degrees
initialLatitude = 30; % latitude in degrees
initialAltitude = 0; % Altitude above Earth's surface in meters
[x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
initialPosition = [x; y; z];
initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame,
% (ignoring the velocity due to the earth's rotation)
else
omegaRotatingFrame = [0;0;pi/5]; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [0.5;0.5;0.5]; % constant acceleration measurement
g = [0;0;0]; % Gravity
initialPosition = [1; 0; 0];% initial position in both frames
initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation)
end
if navFrameRotating == 0
omegaCoriolisIMU = [0;0;0];
else
omegaCoriolisIMU = omegaRotatingFrame;
end
end
% From Wikipedia Angular Velocity page, dr/dt = W*r, where r is
% position vector and W is angular velocity tensor
% We add the initial velocity in the rotating frame because they
% are the same frame at t=0, so no transformation is needed
angularVelocityTensor = [ 0 -omegaRotatingFrame(3) omegaRotatingFrame(2);
omegaRotatingFrame(3) 0 -omegaRotatingFrame(1);
-omegaRotatingFrame(2) omegaRotatingFrame(1) 0 ];
initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity;
initialVelocityRotatingFrame = initialVelocity;
%
currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0
currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition));
currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0
currentVelocityFixedGT = initialVelocityFixedFrame;
currentVelocityRotatingGT = initialVelocityRotatingFrame;
%
epsBias = 1e-20;
sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10);
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10);
sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias);
% Imu metadata
zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero
IMU_metadata.AccelerometerSigma = 1e-5;
IMU_metadata.GyroscopeSigma = 1e-7;
IMU_metadata.IntegrationSigma = 1e-10;
IMU_metadata.BiasAccelerometerSigma = epsBias;
IMU_metadata.BiasGyroscopeSigma = epsBias;
IMU_metadata.BiasAccOmegaInit = epsBias;
%% Initialize storage variables
positionsInFixedGT = zeros(3, length(times));
velocityInFixedGT = zeros(3, length(times));
positionsInRotatingGT = zeros(3, length(times));
velocityInRotatingGT = zeros(3, length(times));
positionsEstimates = zeros(3,length(times));
velocitiesEstimates = zeros(3,length(times));
rotationsErrorVectors = zeros(3,length(times)); % Rotating/Fixed frame selected later
changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step
h = figure;
% Solver object
isamParams = ISAM2Params;
isamParams.setFactorization('CHOLESKY');
isamParams.setRelinearizeSkip(10);
isam = gtsam.ISAM2(isamParams);
newFactors = NonlinearFactorGraph;
newValues = Values;
% Video recording object
if record_movie == 1
writerObj = VideoWriter('trajectories.avi');
writerObj.Quality = 100;
writerObj.FrameRate = 15; %10;
open(writerObj);
set(gca,'nextplot','replacechildren');
set(gcf,'Renderer','zbuffer');
end
%% Print Info about the test
fprintf('\n-------------------------------------------------\n');
if navFrameRotating == 0
fprintf('Performing navigation in the Fixed frame.\n');
else
fprintf('Performing navigation in the Rotating frame.\n');
end
fprintf('Estimation Enabled = %d\n', estimationEnabled);
fprintf('IMU_type = %d\n', IMU_type);
fprintf('Record Movie = %d\n', record_movie);
fprintf('Realistic Values = %d\n', useRealisticValues);
fprintf('deltaT = %f\n', deltaT);
fprintf('timeElapsed = %f\n', timeElapsed);
fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU(2), omegaCoriolisIMU(3));
fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3));
fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3));
fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3));
if(exist('initialLatitude', 'var') && exist('initialLongitude', 'var'))
fprintf('Initial Position\n\t[Long, Lat] = [%f %f] degrees\n\tEFEC = [%f %f %f]\n', ...
initialLongitude, initialLatitude, initialPosition(1), initialPosition(2), initialPosition(3));
else
fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3));
end
fprintf('\n');
%% Main loop: iterate through the ground truth trajectory, add factors
% and values to the factor graph, and perform inference
for i = 1:length(times)
t = times(i);
% Create keys for current state
currentPoseKey = symbol('x', i);
currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i);
%% Set priors on the first iteration
if(i == 1)
% known initial conditions
currentPoseEstimate = currentPoseFixedGT;
if navFrameRotating == 1
currentVelocityEstimate = LieVector(currentVelocityRotatingGT);
else
currentVelocityEstimate = LieVector(currentVelocityFixedGT);
end
% Set Priors
newValues.insert(currentPoseKey, currentPoseEstimate);
newValues.insert(currentVelKey, currentVelocityEstimate);
newValues.insert(currentBiasKey, zeroBias);
% Initial values, same for IMU types 1 and 2
newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x));
newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v));
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b));
% Store data
positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector;
velocityInFixedGT(:,1) = currentVelocityFixedGT;
positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector;
%velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector;
positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector;
currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix;
else
%% Create ground truth trajectory
% Update the position and velocity
% x_t = x_0 + v_0*dt + 1/2*a_0*dt^2
% v_t = v_0 + a_0*dt
currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ...
+ currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT);
currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT;
currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation
% Rotate pose in fixed frame to get pose in rotating frame
previousPositionRotatingGT = currentPoseRotatingGT.translation.vector;
currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame);
inverseCurrentRotatingFrame = (currentRotatingFrame.inverse);
currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT);
currentPositionRotatingGT = currentPoseRotatingGT.translation.vector;
% Get velocity in rotating frame by treating it like a position and using compose
% Maybe Luca knows a better way to do this within GTSAM.
%currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame));
%currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT);
%currentRot3RotatingGT = currentRotatingFrame.rotation();
%currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT));
% TODO: check if initial velocity in rotating frame is correct
currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT;
%currentVelocityRotatingGT = (currentPositionRotatingGT - previousPositionRotatingGT ...
% - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT;
% Store GT (ground truth) poses
positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector;
velocityInFixedGT(:,i) = currentVelocityFixedGT;
positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector;
velocityInRotatingGT(:,i) = currentVelocityRotatingGT;
currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector;
currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix;
%% Estimate trajectory in rotating frame using GTSAM (ground truth measurements)
% Instantiate preintegrated measurements class
if IMU_type == 1
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
zeroBias, ...
IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
IMU_metadata.IntegrationSigma.^2 * eye(3));
elseif IMU_type == 2
currentSummarizedMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements( ...
zeroBias, ...
IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
IMU_metadata.GyroscopeSigma.^2 * eye(3), ...
IMU_metadata.IntegrationSigma.^2 * eye(3), ...
IMU_metadata.BiasAccelerometerSigma.^2 * eye(3), ...
IMU_metadata.BiasGyroscopeSigma.^2 * eye(3), ...
IMU_metadata.BiasAccOmegaInit.^2 * eye(6));
else
error('imuSimulator:coriolisExample:IMU_typeNotFound', ...
'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type);
end
% Add measurement
currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT);
% Add factors to graph
if IMU_type == 1
newFactors.add(ImuFactor( ...
currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ...
currentBiasKey-1, currentSummarizedMeasurement, g, omegaCoriolisIMU));
newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ...
noiseModel.Isotropic.Sigma(6, epsBias)));
newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ...
noiseModel.Isotropic.Sigma(6, epsBias)));
elseif IMU_type == 2
newFactors.add(CombinedImuFactor( ...
currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ...
currentBiasKey-1, currentBiasKey, ...
currentSummarizedMeasurement, g, omegaCoriolisIMU, ...
noiseModel.Isotropic.Sigma(15, epsBias)));
% TODO: prior on biases?
end
% Add values to the graph. Use the current pose and velocity
% estimates as to values when interpreting the next pose and
% velocity estimates
%ImuFactor.Predict(currentPoseEstimate, currentVelocityEstimate, pose_j, vel_j, zeroBias, currentSummarizedMeasurement, g, omegaCoriolisIMU);
newValues.insert(currentPoseKey, currentPoseEstimate);
newValues.insert(currentVelKey, currentVelocityEstimate);
newValues.insert(currentBiasKey, zeroBias);
%newFactors.print(''); newValues.print('');
%% Solve factor graph
if(i > 1 && estimationEnabled)
isam.update(newFactors, newValues);
newFactors = NonlinearFactorGraph;
newValues = Values;
% Get the new pose, velocity, and bias estimates
currentPoseEstimate = isam.calculateEstimate(currentPoseKey);
currentVelocityEstimate = isam.calculateEstimate(currentVelKey);
currentBias = isam.calculateEstimate(currentBiasKey);
positionsEstimates(:,i) = currentPoseEstimate.translation.vector;
velocitiesEstimates(:,i) = currentVelocityEstimate.vector;
biasEstimates(:,i) = currentBias.vector;
% In matrix form: R_error = R_gt'*R_estimate
% Perform Logmap on the rotation matrix to get a vector
if navFrameRotating == 1
rotationError = Rot3(currentPoseRotatingGT.rotation.matrix' * currentPoseEstimate.rotation.matrix);
else
rotationError = Rot3(currentPoseFixedGT.rotation.matrix' * currentPoseEstimate.rotation.matrix);
end
rotationsErrorVectors(:,i) = Rot3.Logmap(rotationError);
end
end
%% incremental plotting for animation (ground truth)
if incrementalPlotting == 1
figure(h)
%plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1)
%hold on;
plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r');
hold on;
plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr');
plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or');
plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g');
plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg');
plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og');
if(estimationEnabled)
plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b');
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb');
plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob');
end
hold off;
xlabel('X axis')
ylabel('Y axis')
zlabel('Z axis')
axis equal
grid on;
%pause(0.1);
% record frames for movie
if record_movie == 1
frame = getframe(gcf);
writeVideo(writerObj,frame);
end
end
end
if record_movie == 1
close(writerObj);
end
% Calculate trajectory length
trajectoryLengthEstimated = 0;
trajectoryLengthFixedFrameGT = 0;
trajectoryLengthRotatingFrameGT = 0;
for i = 2:length(positionsEstimates)
trajectoryLengthEstimated = trajectoryLengthEstimated + norm(positionsEstimates(:,i) - positionsEstimates(:,i-1));
trajectoryLengthFixedFrameGT = trajectoryLengthFixedFrameGT + norm(positionsInFixedGT(:,i) - positionsInFixedGT(:,i-1));
trajectoryLengthRotatingFrameGT = trajectoryLengthRotatingFrameGT + norm(positionsInRotatingGT(:,i) - positionsInRotatingGT(:,i-1));
end
%% Print and plot error results
% Calculate errors for appropriate navigation scheme
if navFrameRotating == 0
axisPositionsError = positionsInFixedGT - positionsEstimates;
axisVelocityError = velocityInFixedGT - velocitiesEstimates;
else
axisPositionsError = positionsInRotatingGT - positionsEstimates;
axisVelocityError = velocityInRotatingGT - velocitiesEstimates;
end
% Plot individual axis position errors
figure
plot(times, axisPositionsError);
plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle);
xlabel('Time [s]');
ylabel('Error (ground_truth - estimate) [m]');
legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST');
% Plot 3D position error
figure
positionError3D = sqrt(axisPositionsError(1,:).^2+axisPositionsError(2,:).^2 + axisPositionsError(3,:).^2);
plot(times, positionError3D);
plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle);
xlabel('Time [s]');
ylabel('3D error [meters]');
% Plot 3D position error
if navFrameRotating == 0
trajLen = trajectoryLengthFixedFrameGT;
else
trajLen = trajectoryLengthRotatingFrameGT;
end
figure
plot(times, (positionError3D/trajLen)*100);
plotTitle = sprintf('3D Error normalized by distance in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle);
xlabel('Time [s]');
ylabel('normalized 3D error [% of distance traveled]');
% Plot individual axis velocity errors
figure
plot(times, axisVelocityError);
plotTitle = sprintf('Axis Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle);
xlabel('Time [s]');
ylabel('Error (ground_truth - estimate) [m/s]');
legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST');
% Plot 3D velocity error
figure
velocityError3D = sqrt(axisVelocityError(1,:).^2+axisVelocityError(2,:).^2 + axisVelocityError(3,:).^2);
plot(times, velocityError3D);
plotTitle = sprintf('3D Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ...
IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3));
title(plotTitle);
xlabel('Time [s]');
ylabel('3D error [meters/s]');
% Plot magnitude of rotation errors
figure
for i = 1:size(rotationsErrorVectors,2)
rotationsErrorMagnitudes(i) = norm(rotationsErrorVectors(:,i));
end
plot(times,rotationsErrorMagnitudes);
title('Rotation Error');
xlabel('Time [s]');
ylabel('Error [rads]');
% Text output for errors
if navFrameRotating == 0
fprintf('Fixed Frame ground truth trajectory length is %f [m]\n', trajectoryLengthFixedFrameGT);
fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated);
fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ...
norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100);
else
fprintf('Rotating Frame ground truth trajectory length is %f [m]\n', trajectoryLengthRotatingFrameGT);
fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated);
fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ...
norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthRotatingFrameGT*100);
end
fprintf('Final velocity error = [%f %f %f] [m/s]\n', axisVelocityError(1,end), axisVelocityError(2,end), axisVelocityError(3,end));
fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end)));
%% Plot final trajectories
figure
[x,y,z] = sphere(30);
if useRealisticValues
mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference
else
mesh(x,y,z);
end
hold on;
axis equal
% Ground truth trajectory in fixed reference frame
plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r','lineWidth',4);
plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr','lineWidth',4);
plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or','lineWidth',4);
% Ground truth trajectory in rotating reference frame
plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g','lineWidth',4);
plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg','lineWidth',4);
plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og','lineWidth',4);
% Estimates
if(estimationEnabled)
plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b','lineWidth',4);
plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb','lineWidth',4);
plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob','lineWidth',4);
end
xlabel('X axis')
ylabel('Y axis')
zlabel('Z axis')
axis equal
grid on;
hold off;
% TODO: logging rotation errors
%for all time steps
% Rerror = Rgt' * Restimated;
% % transforming rotation matrix to axis-angle representation
% vector_error = Rot3.Logmap(Rerror);
% norm(vector_error)
%
% axis angle: [u,theta], with norm(u)=1
% vector_error = u * theta;
% TODO: logging velocity errors
%velocities..

182
matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m Normal file
View File

@ -0,0 +1,182 @@
clc
clear all
close all
% Flag to mark external configuration to the main test script
externalCoriolisConfiguration = 1;
%% General configuration
navFrameRotating = 0; % 0 = perform navigation in the fixed frame
% 1 = perform navigation in the rotating frame
IMU_type = 1; % IMU type 1 or type 2
useRealisticValues = 1; % use reaslist values for initial position and earth rotation
record_movie = 0; % 0 = do not record movie
% 1 = record movie of the trajectories. One
% frame per time step (15 fps)
incrementalPlotting = 0;
estimationEnabled = 1;
%% Scenario Configuration
deltaT = 0.01; % timestep
timeElapsed = 5; % Total elapsed time
times = 0:deltaT:timeElapsed;
% Initial Conditions
omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s)
radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km
if useRealisticValues == 1
omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [0.1;0;1]; % constant acceleration measurement
g = [0;0;0]; % Gravity
initialLongitude = 45; % longitude in degrees
initialLatitude = 30; % latitude in degrees
initialAltitude = 0; % Altitude above Earth's surface in meters
[x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
initialPosition = [x; y; z];
initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame,
% (ignoring the velocity due to the earth's rotation)
else
omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame
omegaFixed = [0;0;0]; % constant rotation rate measurement
accelFixed = [1;0;0]; % constant acceleration measurement
g = [0;0;0]; % Gravity
initialPosition = [0; 1; 0];% initial position in both frames
initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation)
end
%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Run tests with randomly generated initialPosition and accelFixed values
% For each initial position, a number of acceleration vectors are
% generated. For each (initialPosition, accelFixed) pair, the coriolis test
% is run for the following 3 scenarios
% - Navigation performed in fixed frame
% - Navigation performed in rotating frame, including the coriolis effect
% - Navigation performed in rotating frame, ignoring coriolis effect
%
% Testing configuration
numPosTests = 1;
numAccelTests = 10;
totalNumTests = numPosTests * numAccelTests;
% Storage variables
posFinErrorFixed = zeros(numPosTests, numAccelTests);
rotFinErrorFixed = zeros(numPosTests, numAccelTests);
velFinErrorFixed = zeros(numPosTests, numAccelTests);
posFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
rotFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
velFinErrorRotCoriolis = zeros(numPosTests, numAccelTests);
posFinErrorRot = zeros(numPosTests, numAccelTests);
rotFinErrorRot = zeros(numPosTests, numAccelTests);
velFinErrorRot = zeros(numPosTests, numAccelTests);
numErrors = 0;
testIndPos = 1;
testIndAccel = 1;
while testIndPos <= numPosTests
%generate a random initial position vector
initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90)
initialLatitude = rand()*180 - 90; % latitude in degrees (-180 to 180)
initialAltitude = rand()*150; % Altitude above Earth's surface in meters (0-150)
[x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude);
initialPosition = [x; y; z];
while testIndAccel <= numAccelTests
[testIndPos testIndAccel]
% generate a random acceleration vector
accelFixed = 2*rand(3,1)-ones(3,1);
%lla = oldTestInfo(testIndPos,testIndAccel).initialPositionLLA;
%initialLongitude = lla(1);
%initialLatitude = lla(2);
%initialAltitude = lla(3);
%initialPosition = oldTestInfo(testIndPos, testIndAccel).initialPositionECEF;
testInfo(testIndPos, testIndAccel).initialPositionLLA = [initialLongitude, initialLatitude, initialAltitude];
testInfo(testIndPos, testIndAccel).initialPositionECEF = initialPosition;
testInfo(testIndPos, testIndAccel).accelFixed = accelFixed;
try
omegaCoriolisIMU = [0;0;0];
navFrameRotating = 0;
imuSimulator.coriolisExample
posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100;
rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
testInfo(testIndPos, testIndAccel).fixedPositionError = axisPositionsError(:,end);
testInfo(testIndPos, testIndAccel).fixedVelocityError = axisVelocityError(:,end);
testInfo(testIndPos, testIndAccel).fixedRotationError = rotationsErrorVectors(:,end);
testInfo(testIndPos, testIndAccel).fixedEstTrajLength = trajectoryLengthEstimated;
testInfo(testIndPos, testIndAccel).trajLengthFixedFrameGT = trajectoryLengthFixedFrameGT;
testInfo(testIndPos, testIndAccel).trajLengthRotFrameGT = trajectoryLengthRotatingFrameGT;
close all;
% Run the same initial conditions but navigating in the rotating frame
% enable coriolis effect by setting:
omegaCoriolisIMU = omegaRotatingFrame;
navFrameRotating = 1;
imuSimulator.coriolisExample
posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
testInfo(testIndPos, testIndAccel).rotCoriolisPositionError = axisPositionsError(:,end);
testInfo(testIndPos, testIndAccel).rotCoriolisVelocityError = axisVelocityError(:,end);
testInfo(testIndPos, testIndAccel).rotCoriolisRotationError = rotationsErrorVectors(:,end);
testInfo(testIndPos, testIndAccel).rotCoriolisEstTrajLength = trajectoryLengthEstimated;
close all;
% Run the same initial conditions but navigating in the rotating frame
% disable coriolis effect by setting:
omegaCoriolisIMU = [0;0;0];
navFrameRotating = 1;
imuSimulator.coriolisExample
posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100;
rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end));
velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end));
testInfo(testIndPos, testIndAccel).rotPositionError = axisPositionsError(:,end);
testInfo(testIndPos, testIndAccel).rotVelocityError = axisVelocityError(:,end);
testInfo(testIndPos, testIndAccel).rotRotationError = rotationsErrorVectors(:,end);
testInfo(testIndPos, testIndAccel).rotEstTrajLength = trajectoryLengthEstimated;
close all;
catch
numErrors = numErrors + 1;
fprintf('*ERROR*');
fprintf('%d tests cancelled due to errors\n', numErrors);
fprintf('restarting test with new accelFixed');
testIndAccel = testIndAccel - 1;
end
testIndAccel = testIndAccel + 1;
end
testIndAccel = 1;
testIndPos = testIndPos + 1;
end
fprintf('\nTotal: %d tests cancelled due to errors\n', numErrors);
mean(posFinErrorFixed);
max(posFinErrorFixed);
% box(posFinErrorFixed);
mean(posFinErrorRotCoriolis);
max(posFinErrorRotCoriolis);
% box(posFinErrorRotCoriolis);
mean(posFinErrorRot);
max(posFinErrorRot);
% box(posFinErrorRot);

325
matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m Normal file
View File

@ -0,0 +1,325 @@
import gtsam.*;
% Test GTSAM covariances on a factor graph with:
% Between Factors
% IMU factors (type 1 and type 2)
% GPS prior factors on poses
% SmartProjectionPoseFactors
% Authors: Luca Carlone, David Jensen
% Date: 2014/4/6
% Check for an extneral configuration, used when running multiple tests
if ~exist('externallyConfigured', 'var')
clc
clear all
close all
saveResults = 0;
%% Configuration
% General options
options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj
options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses
options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities)
options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1)
options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements
options.includeCameraFactors = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks
options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory)
options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses
options.gpsStartPose = 100; % Pose number to start including GPS factors at
options.trajectoryLength = 100;%209; % length of the ground truth trajectory
options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories)
numMonteCarloRuns = 2; % number of Monte Carlo runs to perform
% Noise values to be adjusted
sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2
sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1
sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3
sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5
sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3
sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5
sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4
sigma_camera = 1; % std. deviation for noise in camera measurements (pixels)
% Set log files
testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro)
folderName = 'results/'
else
fprintf('Tests have been externally configured.\n');
end
%% Between metadata
noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)];
noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose);
%% Imu metadata
metadata.imu.epsBias = 1e-10; % was 1e-7
metadata.imu.g = [0;0;0];
metadata.imu.omegaCoriolis = [0;0;0];
metadata.imu.IntegrationSigma = 1e-5;
metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
metadata.imu.AccelerometerSigma = sigma_accel;
metadata.imu.GyroscopeSigma = sigma_gyro;
metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; % noise on expected change in accelerometer bias over time
metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; % noise on expected change in gyroscope bias over time
% noise on initial accelerometer and gyroscope biases
if options.imuNonzeroBias == 1
metadata.imu.BiasAccOmegaInit = [sigma_accelBias * ones(3,1); sigma_gyroBias * ones(3,1)];
else
metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias * ones(6,1);
end
noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1
noiseBiasBetween = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1);...
metadata.imu.BiasGyroscopeSigma * ones(3,1)]); % between on biases
noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit);
noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1);
noiseVectorGyro = metadata.imu.GyroscopeSigma * ones(3,1);
%% GPS metadata
noiseVectorGPS = sigma_gps * ones(3,1);
noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]);
%% Camera metadata
metadata.camera.calibration = Cal3_S2(500,500,0,1920/2,1200/2); % Camera calibration
metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation
metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation
metadata.camera.zlims = [-30, 30]; % z limits on area for landmark creation
metadata.camera.visualRange = 100; % maximum distance from the camera that a landmark can be seen (meters)
metadata.camera.bodyPoseCamera = Pose3; % pose of camera in body
metadata.camera.CameraSigma = sigma_camera;
cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma);
noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1);
% Create landmarks and smart factors
if options.includeCameraFactors == 1
for i = 1:options.numberOfLandmarks
metadata.camera.gtLandmarkPoints(i) = Point3( ...
[rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ...
rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); ...
rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]);
end
end
%% Create ground truth trajectory and measurements
[gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata);
%% Create ground truth graph
% Set up noise models
gtNoiseModels.noisePose = noisePose;
gtNoiseModels.noiseVel = noiseVel;
gtNoiseModels.noiseBiasBetween = noiseBiasBetween;
gtNoiseModels.noisePriorPose = noisePose;
gtNoiseModels.noisePriorBias = noisePriorBias;
gtNoiseModels.noiseGPS = noiseGPS;
gtNoiseModels.noiseCamera = cameraMeasurementNoise;
% Set measurement noise to 0, because this is ground truth
gtMeasurementNoise.poseNoiseVector = zeros(6,1);
gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1);
gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1);
gtMeasurementNoise.cameraNoiseVector = zeros(2,1);
gtMeasurementNoise.gpsNoiseVector = zeros(3,1);
% Set IMU biases to zero
metadata.imu.accelConstantBiasVector = zeros(3,1);
metadata.imu.gyroConstantBiasVector = zeros(3,1);
[gtGraph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements
gtValues, ... % ground truth Values
gtNoiseModels, ... % noise models to use in this graph
gtMeasurementNoise, ... % noise to apply to measurements
options, ... % options for the graph (e.g. which factors to include)
metadata); % misc data necessary for factor creation
%% Display, printing, and plotting of ground truth
%gtGraph.print(sprintf('\nGround Truth Factor graph:\n'));
%gtValues.print(sprintf('\nGround Truth Values:\n '));
figure(1)
hold on;
if options.includeCameraFactors
b = [-1000 2000 -2000 2000 -30 30];
for i = 1:size(metadata.camera.gtLandmarkPoints,2)
p = metadata.camera.gtLandmarkPoints(i).vector;
if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6))
plot3(p(1), p(2), p(3), 'k+');
end
end
pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0));
for i = 1:length(pointsToPlot)
p = pointsToPlot(i).vector;
plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10);
end
end
plot3DPoints(gtValues);
%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues));
plot3DTrajectory(gtValues, '-r');
axis equal
% optimize
optimizer = GaussNewtonOptimizer(gtGraph, gtValues);
gtEstimate = optimizer.optimize();
plot3DTrajectory(gtEstimate, '-k');
% estimate should match gtValues if graph is correct.
fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) );
fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) );
disp('Plotted ground truth')
%% Monte Carlo Runs
% Set up noise models
monteCarloNoiseModels.noisePose = noisePose;
monteCarloNoiseModels.noiseVel = noiseVel;
monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween;
monteCarloNoiseModels.noisePriorPose = noisePose;
monteCarloNoiseModels.noisePriorBias = noisePriorBias;
monteCarloNoiseModels.noiseGPS = noiseGPS;
monteCarloNoiseModels.noiseCamera = cameraMeasurementNoise;
% Set measurement noise for monte carlo runs
monteCarloMeasurementNoise.poseNoiseVector = zeros(6,1); %noiseVectorPose;
monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel;
monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro;
monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS;
monteCarloMeasurementNoise.cameraNoiseVector = noiseVectorCamera;
for k=1:numMonteCarloRuns
fprintf('Monte Carlo Run %d...\n', k');
% Create a random bias for each run
if options.imuNonzeroBias == 1
metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1);
metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1);
%metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1);
%metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1);
else
metadata.imu.accelConstantBiasVector = zeros(3,1);
metadata.imu.gyroConstantBiasVector = zeros(3,1);
end
% Create a new graph using noisy measurements
[graph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ...
gtMeasurements, ... % ground truth measurements
gtValues, ... % ground truth Values
monteCarloNoiseModels, ... % noise models to use in this graph
monteCarloMeasurementNoise, ... % noise to apply to measurements
options, ... % options for the graph (e.g. which factors to include)
metadata); % misc data necessary for factor creation
%graph.print('graph')
% optimize
optimizer = GaussNewtonOptimizer(graph, gtValues);
estimate = optimizer.optimize();
figure(1)
plot3DTrajectory(estimate, '-b');
marginals = Marginals(graph, estimate);
% for each pose in the trajectory
for i=0:options.trajectoryLength
% compute estimation errors
currentPoseKey = symbol('x', i);
gtPosition = gtValues.at(currentPoseKey).translation.vector;
estPosition = estimate.at(currentPoseKey).translation.vector;
estR = estimate.at(currentPoseKey).rotation.matrix;
errPosition = estPosition - gtPosition;
% compute covariances:
cov = marginals.marginalCovariance(currentPoseKey);
covPosition = estR * cov(4:6,4:6) * estR';
% compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances
NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof
end
figure(2)
hold on
plot(NEES(k,:),'-b','LineWidth',1.5)
end
%%
ANEES = mean(NEES);
plot(ANEES,'-r','LineWidth',2)
plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof
box on
set(gca,'Fontsize',16)
title('NEES and ANEES');
if saveResults
saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig');
saveas(gcf,horzcat(folderName,'runs-',testName,'.png'),'png');
end
%%
figure(1)
box on
set(gca,'Fontsize',16)
title('Ground truth and estimates for each MC runs');
if saveResults
saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig');
saveas(gcf,horzcat(folderName,'gt-',testName,'.png'),'png');
end
%% Let us compute statistics on the overall NEES
n = 3; % position vector dimension
N = numMonteCarloRuns; % number of runs
alpha = 0.01; % confidence level
% mean_value = n*N; % mean value of the Chi-square distribution
% (we divide by n * N and for this reason we expect ANEES around 1)
r1 = chi2inv(alpha, n * N) / (n * N);
r2 = chi2inv(1-alpha, n * N) / (n * N);
% output here
fprintf(1, 'r1 = %g\n', r1);
fprintf(1, 'r2 = %g\n', r2);
figure(3)
hold on
plot(ANEES/n,'-b','LineWidth',2)
plot(ones(size(ANEES,2),1),'r-');
plot(r1*ones(size(ANEES,2),1),'k-.');
plot(r2*ones(size(ANEES,2),1),'k-.');
box on
set(gca,'Fontsize',16)
title('NEES normalized by dof VS bounds');
if saveResults
saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig');
saveas(gcf,horzcat(folderName,'ANEES-',testName,'.png'),'png');
logFile = horzcat(folderName,'log-',testName);
save(logFile)
end
%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4)
% the nees for a single experiment (i) is defined as
% NEES_i = xtilda' * inv(P) * xtilda,
% where xtilda in R^n is the estimation
% error, and P is the covariance estimated by the approach we want to test
%
% Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average
% NEES is:
% ANEES = sum(NEES_i)/N
% The quantity N*ANEES is distributed according to a Chi-square
% distribution with N*n degrees of freedom.
%
% For the single run case, N=1, therefore NEES = ANEES is distributed
% according to a chi-square distribution with n degrees of freedom (e.g. n=3
% if we are testing a position estimate)
% Therefore its mean should be n (difficult to see from a single run)
% and, with probability alpha, it should hold:
%
% NEES in [r1, r2]
%
% where r1 and r2 are built from the Chi-square distribution

199
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m Normal file
View File

@ -0,0 +1,199 @@
function [ graph projectionFactorSeenBy] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata)
% Create a factor graph based on provided measurements, values, and noises.
% Used for covariance analysis scripts.
% 'options' contains fields for including various factor types.
% 'metadata' is a storage variable for miscellaneous factor-specific values
% Authors: Luca Carlone, David Jensen
% Date: 2014/04/16
import gtsam.*;
graph = NonlinearFactorGraph;
% Iterate through the trajectory
for i=0:length(measurements)
% Get the current pose
currentPoseKey = symbol('x', i);
currentPose = values.at(currentPoseKey);
if i==0
%% first time step, add priors
% Pose prior (poses used for all factors)
noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* randn(6,1);
initialPose = Pose3.Expmap(noisyInitialPoseVector);
graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose));
% IMU velocity and bias priors
if options.includeIMUFactors == 1
currentVelKey = symbol('v', 0);
currentVel = values.at(currentVelKey).vector;
graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel));
currentBiasKey = symbol('b', 0);
currentBias = values.at(currentBiasKey);
graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias));
end
%% Create a SmartProjectionFactor for each landmark
projectionFactorSeenBy = [];
if options.includeCameraFactors == 1
for j=1:options.numberOfLandmarks
SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01);
% Use constructor with default values, but express the pose of the
% camera as a 90 degree rotation about the X axis
% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ...
% 1, ... % rankTol
% -1, ... % linThreshold
% false, ... % manageDegeneracy
% false, ... % enableEPI
% metadata.camera.bodyPoseCamera); % Pose of camera in body frame
end
projectionFactorSeenBy = zeros(options.numberOfLandmarks,1);
end
else
%% Add Between factors
if options.includeBetweenFactors == 1
prevPoseKey = symbol('x', i-1);
% Create the noisy pose estimate
deltaPose = Pose3.Expmap(measurements(i).deltaVector ...
+ (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise
% Add the between factor to the graph
graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose));
end % end of Between pose factor creation
%% Add IMU factors
if options.includeIMUFactors == 1
% Update keys
currentVelKey = symbol('v', i); % not used if includeIMUFactors is false
currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false
if options.imuFactorType == 1
use2ndOrderIntegration = true;
% Initialize preintegration
imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(...
metadata.imu.zeroBias, ...
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
metadata.imu.IntegrationSigma.^2 * eye(3), ...
use2ndOrderIntegration);
% Generate IMU measurements with noise
for j=1:length(measurements(i).imu) % all measurements to preintegrate
accelNoise = (measurementNoise.imu.accelNoiseVector .* randn(3,1));
imuAccel = measurements(i).imu(j).accel ...
+ accelNoise ... % added noise
+ metadata.imu.accelConstantBiasVector; % constant bias
gyroNoise = (measurementNoise.imu.gyroNoiseVector .* randn(3,1));
imuGyro = measurements(i).imu(j).gyro ...
+ gyroNoise ... % added noise
+ metadata.imu.gyroConstantBiasVector; % constant bias
% Used for debugging
%fprintf(' A: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ...
% measurements(i).imu(j).accel(1), measurements(i).imu(j).accel(2), measurements(i).imu(j).accel(3), ...
% accelNoise(1), accelNoise(2), accelNoise(3), ...
% metadata.imu.accelConstantBiasVector(1), metadata.imu.accelConstantBiasVector(2), metadata.imu.accelConstantBiasVector(3), ...
% imuAccel(1), imuAccel(2), imuAccel(3));
%fprintf(' G: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ...
% measurements(i).imu(j).gyro(1), measurements(i).imu(j).gyro(2), measurements(i).imu(j).gyro(3), ...
% gyroNoise(1), gyroNoise(2), gyroNoise(3), ...
% metadata.imu.gyroConstantBiasVector(1), metadata.imu.gyroConstantBiasVector(2), metadata.imu.gyroConstantBiasVector(3), ...
% imuGyro(1), imuGyro(2), imuGyro(3));
% Preintegrate
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
end
%imuMeasurement.print('imuMeasurement');
% Add Imu factor
graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ...
currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis));
% Add between factor on biases
graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ...
noiseModels.noiseBiasBetween));
end
if options.imuFactorType == 2
use2ndOrderIntegration = true;
% Initialize preintegration
imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(...
metadata.imu.zeroBias, ...
metadata.imu.AccelerometerSigma.^2 * eye(3), ...
metadata.imu.GyroscopeSigma.^2 * eye(3), ...
metadata.imu.IntegrationSigma.^2 * eye(3), ...
metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... % how bias changes over time
metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... % how bias changes over time
diag(metadata.imu.BiasAccOmegaInit.^2), ... % prior on bias
use2ndOrderIntegration);
% Generate IMU measurements with noise
for j=1:length(measurements(i).imu) % all measurements to preintegrate
imuAccel = measurements(i).imu(j).accel ...
+ (measurementNoise.imu.accelNoiseVector .* randn(3,1))... % added noise
+ metadata.imu.accelConstantBiasVector; % constant bias
imuGyro = measurements(i).imu(j).gyro ...
+ (measurementNoise.imu.gyroNoiseVector .* randn(3,1))... % added noise
+ metadata.imu.gyroConstantBiasVector; % constant bias
% Preintegrate
imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT);
end
% Add Imu factor
graph.add(CombinedImuFactor( ...
currentPoseKey-1, currentVelKey-1, ...
currentPoseKey, currentVelKey, ...
currentBiasKey-1, currentBiasKey, ...
imuMeasurement, ...
metadata.imu.g, metadata.imu.omegaCoriolis));
end
end % end of IMU factor creation
%% Build Camera Factors
if options.includeCameraFactors == 1
for j = 1:length(measurements(i).landmarks)
cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1);
cameraPixelMeasurement = measurements(i).landmarks(j).vector;
% Only add the measurement if it is in the image frame (based on calibration)
if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ...
&& cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ...
&& cameraPixelMeasurement(2) < 2*metadata.camera.calibration.py)
cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise;
SmartProjectionFactors(j).add( ...
Point2(cameraPixelMeasurement), ...
currentPoseKey, noiseModels.noiseCamera, ...
metadata.camera.calibration);
projectionFactorSeenBy(j) = projectionFactorSeenBy(j)+1;
end
end
end % end of Camera factor creation
%% Add GPS factors
if options.includeGPSFactors == 1 && i >= options.gpsStartPose
gpsPosition = measurements(i).gpsPositionVector ...
+ measurementNoise.gpsNoiseVector .* randn(3,1);
graph.add(PriorFactorPose3(currentPoseKey, ...
Pose3(currentPose.rotation, Point3(gpsPosition)), ...
noiseModels.noiseGPS));
end % end of GPS factor creation
end % end of else (i=0)
end % end of for over trajectory
%% Add Camera Factors to the graph
% Only factors for landmarks that have been viewed at least once are added
% to the graph
%[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))]
if options.includeCameraFactors == 1
for j = 1:options.numberOfLandmarks
if projectionFactorSeenBy(j) > 0
graph.add(SmartProjectionFactors(j));
end
end
end
end % end of function

150
matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m Normal file
View File

@ -0,0 +1,150 @@
function [values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata )
% Create a trajectory for running covariance analysis scripts.
% 'options' contains fields for including various factor types and setting trajectory length
% 'metadata' is a storage variable for miscellaneous factor-specific values
% Authors: Luca Carlone, David Jensen
% Date: 2014/04/16
import gtsam.*;
values = Values;
warning('Rotating Pose inside getPoseFromGtScenario! TODO: Use a body_P_sensor transform in the factors')
if options.useRealData == 1
%% Create a ground truth trajectory from Real data (if available)
fprintf('\nUsing real data as ground truth\n');
gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',...
'VEast', 'VNorth', 'VUp');
% Limit the trajectory length
options.trajectoryLength = min([length(gtScenario.Lat)/options.subsampleStep options.trajectoryLength]);
fprintf('Scenario Ind: ');
for i=0:options.trajectoryLength
scenarioInd = options.subsampleStep * i + 1;
fprintf('%d, ', scenarioInd);
if (mod(i,12) == 0) fprintf('\n'); end
%% Generate the current pose
currentPoseKey = symbol('x', i);
currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd);
%% FOR TESTING - this is currently moved to getPoseFromGtScenario
%currentPose = currentPose.compose(metadata.camera.bodyPoseCamera);
%currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0]));
% add to values
values.insert(currentPoseKey, currentPose);
%% gt Between measurements
if options.includeBetweenFactors == 1 && i > 0
prevPose = values.at(currentPoseKey - 1);
deltaPose = prevPose.between(currentPose);
measurements(i).deltaVector = Pose3.Logmap(deltaPose);
end
%% gt IMU measurements
if options.includeIMUFactors == 1
currentVelKey = symbol('v', i);
currentBiasKey = symbol('b', i);
deltaT = 1; % amount of time between IMU measurements
if i == 0
currentVel = [0 0 0]';
else
% integrate & store intermediate measurements
for j=1:options.subsampleStep % we integrate all the intermediate measurements
previousScenarioInd = options.subsampleStep * (i-1) + 1;
scenarioIndIMU1 = previousScenarioInd+j-1;
scenarioIndIMU2 = previousScenarioInd+j;
IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1);
IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2);
IMURot1 = IMUPose1.rotation.matrix;
IMUdeltaPose = IMUPose1.between(IMUPose2);
IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose);
IMUdeltaRotVector = IMUdeltaPoseVector(1:3);
IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame
measurements(i).imu(j).deltaT = deltaT;
% gyro rate: Logmap(R_i' * R_i+1) / deltaT
measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT;
% deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT;
% acc = (deltaPosition - initialVel * dT) * (2/dt^2)
measurements(i).imu(j).accel = IMURot1' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT));
% Update velocity
currentVel = currentVel + IMURot1 * measurements(i).imu(j).accel * deltaT;
end
end
% Add Values: velocity and bias
values.insert(currentVelKey, LieVector(currentVel));
values.insert(currentBiasKey, metadata.imu.zeroBias);
end
%% gt GPS measurements
if options.includeGPSFactors == 1 && i > 0
gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector;
measurements(i).gpsPositionVector = gpsPositionVector;
end
%% gt Camera measurements
if options.includeCameraFactors == 1 && i > 0
% Create the camera based on the current pose and the pose of the
% camera in the body
cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera);
camera = SimpleCamera(cameraPose, metadata.camera.calibration);
%camera = SimpleCamera(currentPose, metadata.camera.calibration);
% Record measurements if the landmark is within visual range
for j=1:length(metadata.camera.gtLandmarkPoints)
distanceToLandmark = camera.pose.range(metadata.camera.gtLandmarkPoints(j));
if distanceToLandmark < metadata.camera.visualRange
try
z = camera.project(metadata.camera.gtLandmarkPoints(j));
measurements(i).landmarks(j) = z;
catch
% point is probably out of the camera's view
end
end
end
end
end
fprintf('\n');
else
error('Please use RealData')
%% Create a random trajectory as ground truth
currentPose = Pose3; % initial pose % initial pose
unsmooth_DP = 0.5; % controls smoothness on translation norm
unsmooth_DR = 0.1; % controls smoothness on rotation norm
fprintf('\nCreating a random ground truth trajectory\n');
currentPoseKey = symbol('x', 0);
values.insert(currentPoseKey, currentPose);
for i=1:options.trajectoryLength
% Update the pose key
currentPoseKey = symbol('x', i);
% Generate the new measurements
gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0]
gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0]
measurements.deltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition];
% Create the next pose
deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)');
currentPose = currentPose.compose(deltaPose);
% Add the current pose as a value
values.insert(currentPoseKey, currentPose);
end % end of random trajectory creation
end % end of else
end % end of function

55
matlab/unstable_examples/+imuSimulator/ct2ENU.m Normal file
View File

@ -0,0 +1,55 @@
function [dx,dy,dz]=ct2ENU(dX,dY,dZ,lat,lon)
% CT2LG Converts CT coordinate differences to local geodetic.
% Local origin at lat,lon,h. If lat,lon are vectors, dx,dy,dz
% are referenced to orgin at lat,lon of same index. If
% astronomic lat,lon input, output is in local astronomic
% system. Vectorized in both dx,dy,dz and lat,lon. See also
% LG2CT.
% Version: 2011-02-19
% Useage: [dx,dy,dz]=ct2lg(dX,dY,dZ,lat,lon)
% Input: dX - vector of X coordinate differences in CT
% dY - vector of Y coordinate differences in CT
% dZ - vector of Z coordinate differences in CT
% lat - lat(s) of local system origin (rad); may be vector
% lon - lon(s) of local system origin (rad); may be vector
% Output: dx - vector of x coordinates in local system (east)
% dy - vector of y coordinates in local system (north)
% dz - vector of z coordinates in local system (up)
% Copyright (c) 2011, Michael R. Craymer
% All rights reserved.
% Email: mike@craymer.com
if nargin ~= 5
warning('Incorrect number of input arguements');
return
end
n=length(dX);
if length(lat)==1
lat=ones(n,1)*lat;
lon=ones(n,1)*lon;
end
R=zeros(3,3,n);
R(1,1,:)=-sin(lat').*cos(lon');
R(1,2,:)=-sin(lat').*sin(lon');
R(1,3,:)=cos(lat');
R(2,1,:)=sin(lon');
R(2,2,:)=-cos(lon');
R(2,3,:)=zeros(1,n);
R(3,1,:)=cos(lat').*cos(lon');
R(3,2,:)=cos(lat').*sin(lon');
R(3,3,:)=sin(lat');
RR=reshape(R(1,:,:),3,n);
dx_temp=sum(RR'.*[dX dY dZ],2);
RR=reshape(R(2,:,:),3,n);
dy_temp=sum(RR'.*[dX dY dZ],2);
RR=reshape(R(3,:,:),3,n);
dz=sum(RR'.*[dX dY dZ],2);
dx = -dy_temp;
dy = dx_temp;

39
matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m Normal file
View File

@ -0,0 +1,39 @@
function currentPose = getPoseFromGtScenario(gtScenario,scenarioInd)
% gtScenario contains vectors (Lat, Lon, Alt, Roll, Pitch, Yaw)
% The function picks the index 'scenarioInd' in those vectors and
% computes the corresponding pose by
% 1) Converting (Lat,Lon,Alt) to local coordinates
% 2) Converting (Roll,Pitch,Yaw) to a rotation matrix
import gtsam.*;
Org_lat = gtScenario.Lat(1);
Org_lon = gtScenario.Lon(1);
initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]);
gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]);
% truth in ENU
dX = gtECEF(1) - initialPositionECEF(1);
dY = gtECEF(2) - initialPositionECEF(2);
dZ = gtECEF(3) - initialPositionECEF(3);
[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon);
gtPosition = Point3([xlt, ylt, zlt]');
% use the gtsam.Rot3.Ypr constructor (yaw, pitch, roll) from the ground truth data
% yaw = measured positively to the right
% pitch = measured positively up
% roll = measured positively to right
% Assumes vehice X forward, Y right, Z down
%
% In the gtScenario data
% heading (yaw) = measured positively to the left from Y-axis
% pitch =
% roll =
% Coordinate frame is Y forward, X is right, Z is up
gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd));
currentPose = Pose3(gtBodyRotation, gtPosition);
%% Rotate the pose
currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0]));
end

17
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m Normal file
View File

@ -0,0 +1,17 @@
function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ...
initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT)
%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement
import gtsam.*;
imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector;
finalPosition = Point3(initialPoseGlobal.translation.vector ...
+ initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
finalPose = Pose3(finalRotation, finalPosition);
end

26
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m Normal file
View File

@ -0,0 +1,26 @@
function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame( ...
initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT, velocity1Body)
% Before integrating in the body frame we need to compensate for the Coriolis
% effect
acc_body = acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector;
% after compensating for coriolis this will be essentially zero
% since we are moving at constant body velocity
import gtsam.*;
%% Integrate in the body frame
% Integrate rotations
imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
% Integrate positions
finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT;
finalVelocityBody = velocity1Body + acc_body * deltaT;
%% Express the integrated quantities in the global frame
finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() );
finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ;
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
% Include position and rotation in a pose
finalPose = Pose3(finalRotation, Point3(finalPosition) );
end

21
matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m Normal file
View File

@ -0,0 +1,21 @@
function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_navFrame( ...
initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT)
%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement
import gtsam.*;
% Integrate rotations
imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT);
finalRotation = initialPoseGlobal.rotation.compose(imu2in1);
intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 ));
% Integrate positions (equation (1) in Lupton)
accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector;
finalPosition = Point3(initialPoseGlobal.translation.vector ...
+ initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
% Include position and rotation in a pose
finalPose = Pose3(finalRotation, finalPosition);
end

24
matlab/unstable_examples/+imuSimulator/integrateTrajectory.m Normal file
View File

@ -0,0 +1,24 @@
function [ finalPose, finalVelocityGlobal ] = integrateTrajectory( ...
initialPose, omega1Body, velocity1Body, velocity2Body, deltaT)
%INTEGRATETRAJECTORY Integrate one trajectory step
import gtsam.*;
% Rotation: R^1_2
body2in1 = Rot3.Expmap(omega1Body * deltaT);
% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2
velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector;
% Acceleration: a^1 = (v^1_2 - v^1_1)/dt
accelBody1 = (velocity2inertial - velocity1Body) / deltaT;
% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1
initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector;
% Acceleration in frame W: a^W = R^W_1 a^1
accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector;
finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT);
finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT;
finalRotation = initialPose.rotation.compose(body2in1);
finalPose = Pose3(finalRotation, finalPosition);
end

173
matlab/unstable_examples/+imuSimulator/runConsistencyTests.m Normal file
View File

@ -0,0 +1,173 @@
% use this script to easily run and save results for multiple consistency
% tests without having to pay attention to the computer every 5 minutes
import gtsam.*;
resultsDir = 'results/'
if (~exist(resultsDir, 'dir'))
mkdir(resultsDir);
end
testOptions = [ ...
% 1 2 3 4 5 6 7 8 9 10 11 12
% RealData? Between? IMU? IMUType Bias? Camera? #LndMrk GPS? StrtPose TrajLength Subsample #MCRuns
%1 0 1 2 0 0 100 0 100 209 20 100 ;... % 1
%1 0 1 2 0 0 100 0 100 209 20 100 ;... % 2
% 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 3
1 0 1 2 0 1 100 0 100 209 20 20 ;... % 4
1 0 1 2 0 1 100 0 100 209 20 20 ;... % 5
1 0 1 2 0 0 100 0 100 209 20 20 ];%... % 6
% 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 7
%1 0 1 2 0 0 100 0 100 209 20 1 ;... % 8
%1 0 1 2 0 0 100 0 100 209 20 1 ]; % 9
noises = [ ...
% 1 2 3 4 5 6 7 8
% sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps sigma_camera
%1e-2 1e-1 1e-3 1e-5 0 0 1e-4 1;... % 1
%1e-2 1e-1 1e-2 1e-5 0 0 1e-4 1;... % 2
% 1e-2 1e-1 1e-1 1e-5 0 0 1e-4 1;... % 3
1e-2 1e-1 1e-3 1e-4 0 0 1e-4 1;... % 4
1e-2 1e-1 1e-3 1e-3 0 0 1e-4 1;... % 5
1e-2 1e-1 1e-3 1e-2 0 0 1e-4 1];%... % 6
% 1e-2 1e-1 1e-3 1e-1 0 0 1e-4 1;... % 7
%1e-2 1e-1 1e-3 1e-2 1e-3 1e-5 1e-4 1;... % 8
%1e-2 1e-1 1e-3 1e-2 1e-4 1e-6 1e-4 1]; % 9
if(size(testOptions,1) ~= size(noises,1))
error('testOptions and noises do not have same number of rows');
end
% Set flag so the script knows there is an external configuration
externallyConfigured = 1;
% Set the flag to save the results
saveResults = 0;
errorRuns = [];
% Go through tests
for i = 1:size(testOptions,1)
% Clean up from last test
close all;
%clc;
% Set up variables for test
options.useRealData = testOptions(i,1);
options.includeBetweenFactors = testOptions(i,2);
options.includeIMUFactors = testOptions(i,3);
options.imuFactorType = testOptions(i,4);
options.imuNonzeroBias = testOptions(i,5);
options.includeCameraFactors = testOptions(i,6);
options.numberOfLandmarks = testOptions(i,7);
options.includeGPSFactors = testOptions(i,8);
options.gpsStartPose = testOptions(i,9);
options.trajectoryLength = testOptions(i,10);
options.subsampleStep = testOptions(i,11);
numMonteCarloRuns = testOptions(i,12);
sigma_ang = noises(i,1);
sigma_cart = noises(i,2);
sigma_accel = noises(i,3);
sigma_gyro = noises(i,4);
sigma_accelBias = noises(i,5);
sigma_gyroBias = noises(i,6);
sigma_gps = noises(i,7);
sigma_camera = noises(i,8);
% Create folder name
f_between = '';
f_imu = '';
f_bias = '';
f_gps = '';
f_camera = '';
f_runs = '';
if (options.includeBetweenFactors == 1);
f_between = 'between_';
end
if (options.includeIMUFactors == 1)
f_imu = sprintf('imu%d_', options.imuFactorType);
if (options.imuNonzeroBias == 1);
f_bias = sprintf('bias_a%1.2g_g%1.2g_', sigma_accelBias, sigma_gyroBias);
end
end
if (options.includeGPSFactors == 1);
f_between = sprintf('gps_%d_', gpsStartPose);
end
if (options.includeCameraFactors == 1)
f_camera = sprintf('camera_%d_', options.numberOfLandmarks);
end
f_runs = sprintf('mc%d', numMonteCarloRuns);
folderName = [resultsDir f_between f_imu f_bias f_gps f_camera f_runs '/'];
% make folder if it doesnt exist
if (~exist(folderName, 'dir'))
mkdir(folderName);
end
testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro);
% Run the test
fprintf('Test %d\n\tResults will be saved to:\n\t%s\n\trunning...\n', i, folderName);
fprintf('Test Name: %s\n', testName);
try
imuSimulator.covarianceAnalysisBetween;
catch
errorRuns = [errorRuns i];
fprintf('\n*****\n Something went wrong, most likely indeterminant linear system error.\n');
disp('Test Options:\n');
disp(testOptions(i,:));
disp('Noises');
disp(noises(i,:));
fprintf('\n*****\n\n');
end
end
% Print error summary
fprintf('*************************\n');
fprintf('%d Runs failed due to errors (data not collected for failed runs)\n', length(errorRuns));
for i = 1:length(errorRuns)
k = errorRuns(i);
fprintf('\nTest %d:\n', k);
fprintf(' options.useRealData = %d\n', testOptions(k,1));
fprintf(' options.includeBetweenFactors = %d\n', testOptions(k,2));
fprintf(' options.includeIMUFactors = %d\n', testOptions(k,3));
fprintf(' options.imuFactorType = %d\n', testOptions(k,4));
fprintf(' options.imuNonzeroBias = %d\n', testOptions(k,5));
fprintf(' options.includeCameraFactors = %d\n', testOptions(k,6));
fprintf(' numberOfLandmarks = %d\n', testOptions(k,7));
fprintf(' options.includeGPSFactors = %d\n', testOptions(k,8));
fprintf(' options.gpsStartPose = %d\n', testOptions(k,9));
fprintf(' options.trajectoryLength = %d\n', testOptions(k,10));
fprintf(' options.subsampleStep = %d\n', testOptions(k,11));
fprintf(' numMonteCarloRuns = %d\n', testOptions(k,12));
fprintf('\n');
fprintf(' sigma_ang = %f\n', noises(i,1));
fprintf(' sigma_cart = %f\n', noises(i,2));
fprintf(' sigma_accel = %f\n', noises(i,3));
fprintf(' sigma_gyro = %f\n', noises(i,4));
fprintf(' sigma_accelBias = %f\n', noises(i,5));
fprintf(' sigma_gyroBias = %f\n', noises(i,6));
fprintf(' sigma_gps = %f\n', noises(i,7));
end

48
matlab/unstable_examples/+imuSimulator/test1onestep.m Normal file
View File

@ -0,0 +1,48 @@
import gtsam.*;
deltaT = 0.01;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant global velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant global velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
R = Rot3.Expmap(omega * deltaT);
velocity2body = R.unrotate(Point3(velocity)).vector;
acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1];
acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0])));
disp('IMU measurement discrepancy:');
disp(acc_omegaActual - acc_omegaExpected);
initialPose = Pose3;
finalPoseExpected = Pose3(Rot3.Expmap(omega * deltaT), Point3(velocity * deltaT));
finalVelocityExpected = velocity;
[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity2body, deltaT);
disp('Final pose discrepancy:');
disp(finalPoseExpected.between(finalPoseActual).matrix);
disp('Final velocity discrepancy:');
disp(finalVelocityActual - finalVelocityExpected);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant body velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant body velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
acc_omegaExpected = [-0.01; 0.1; 0; 0; 0; 0.1];
acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity, deltaT, Pose3(Rot3, Point3([1;0;0])));
disp('IMU measurement discrepancy:');
disp(acc_omegaActual - acc_omegaExpected);
initialPose = Pose3;
initialVelocity = velocity;
finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT);
finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector;
[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT);
disp('Final pose discrepancy:');
disp(finalPoseExpected.between(finalPoseActual).matrix);
disp('Final velocity discrepancy:');
disp(finalVelocityActual - finalVelocityExpected);

34
matlab/unstable_examples/+imuSimulator/test2constglobal.m Normal file
View File

@ -0,0 +1,34 @@
import gtsam.*;
deltaT = 0.01;
timeElapsed = 1000;
times = 0:deltaT:timeElapsed;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant global velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant global velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
% Initial state
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Positions
positions = zeros(3, length(times)+1);
i = 2;
for t = times
velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector;
R = Rot3.Expmap(omega * deltaT);
velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector;
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT);
positions(:,i) = currentPoseGlobal.translation.vector;
i = i + 1;
end
figure;
plot(positions(1,:), positions(2,:), '.-');

96
matlab/unstable_examples/+imuSimulator/test3constbody.m Normal file
View File

@ -0,0 +1,96 @@
clc
clear all
close all
import gtsam.*;
addpath(genpath('./Libraries'))
deltaT = 0.01;
timeElapsed = 25;
times = 0:deltaT:timeElapsed;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Constant body velocity w/ lever arm
disp('--------------------------------------------------------');
disp('Constant body velocity w/ lever arm');
omega = [0;0;0.1];
velocity = [1;0;0];
RIMUinBody = Rot3.Rz(-pi/2);
% RIMUinBody = eye(3)
IMUinBody = Pose3(RIMUinBody, Point3([1;0;0]));
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Initial state (IMU)
currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody);
%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here?
currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ...
Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector;
% Positions
% body trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Expected IMU trajectory (from body trajectory and lever arm)
positionsIMUe = zeros(3, length(times)+1);
positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
posesIMUe(1).p = positionsIMUe(:,1);
posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix;
% Integrated IMU trajectory (from IMU measurements)
positionsIMU = zeros(3, length(times)+1);
positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
posesIMU(1).p = positionsIMU(:,1);
posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;
i = 2;
for t = times
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
acc_omega = imuSimulator.calculateIMUMeasurement( ...
omega, omega, velocity, velocity, deltaT, IMUinBody);
[ currentPoseGlobalIMU, currentVelocityGlobalIMU ] = imuSimulator.integrateIMUTrajectory( ...
currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT);
% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector;
positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector;
poses(i).p = positions(:,i);
posesIMUe(i).p = positionsIMUe(:,i);
posesIMU(i).p = positionsIMU(:,i);
poses(i).R = currentPoseGlobal.rotation.matrix;
posesIMUe(i).R = poses(i).R * IMUinBody.rotation.matrix;
posesIMU(i).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix;
i = i + 1;
end
figure(1)
plot_trajectory(poses, 50, '-k', 'body trajectory',0.1,0.75,1)
hold on
plot_trajectory(posesIMU, 50, '-r', 'imu trajectory',0.1,0.75,1)
figure(2)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot(positionsIMU(1,:), positionsIMU(2,:), '-r');
plot(positionsIMUe(1,:), positionsIMUe(2,:), ':k');
axis equal;
disp('Mismatch between final integrated IMU position and expected IMU position')
disp(norm(posesIMUe(end).p - posesIMU(end).p))
disp('Mismatch between final integrated IMU orientation and expected IMU orientation')
disp(norm(posesIMUe(end).R - posesIMU(end).R))

97
matlab/unstable_examples/+imuSimulator/test4circle.m Normal file
View File

@ -0,0 +1,97 @@
clc
clear all
close all
import gtsam.*;
deltaT = 0.001;
timeElapsed = 1;
times = 0:deltaT:timeElapsed;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
disp('--------------------------------------------------------');
disp('Integration in body frame VS integration in navigation frame');
disp('TOY EXAMPLE:');
disp('- Body moves along a circular trajectory with constant rotation rate -omega- and constant -velocity- (in the body frame)');
disp('- We compare two integration schemes: integating in the navigation frame (similar to Lupton approach) VS integrating in the body frame')
disp('- Navigation frame is assumed inertial for simplicity')
omega = [0;0;2*pi];
velocity = [1;0;0];
%% Set initial conditions for the true trajectory and for the estimates
% (one estimate is obtained by integrating in the body frame, the other
% by integrating in the navigation frame)
% Initial state (body)
currentPoseGlobal = Pose3;
currentVelocityGlobal = velocity;
% Initial state estimate (integrating in navigation frame)
currentPoseGlobalIMUnav = currentPoseGlobal;
currentVelocityGlobalIMUnav = currentVelocityGlobal;
% Initial state estimate (integrating in the body frame)
currentPoseGlobalIMUbody = currentPoseGlobal;
currentVelocityGlobalIMUbody = currentVelocityGlobal;
%% Prepare data structures for actual trajectory and estimates
% Actual trajectory
positions = zeros(3, length(times)+1);
positions(:,1) = currentPoseGlobal.translation.vector;
poses(1).p = positions(:,1);
poses(1).R = currentPoseGlobal.rotation.matrix;
% Trajectory estimate (integrated in the navigation frame)
positionsIMUnav = zeros(3, length(times)+1);
positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUnav(1).p = positionsIMUnav(:,1);
posesIMUnav(1).R = poses(1).R;
% Trajectory estimate (integrated in the body frame)
positionsIMUbody = zeros(3, length(times)+1);
positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector;
posesIMUbody(1).p = positionsIMUbody(:,1);
posesIMUbody(1).R = poses(1).R;
%% Main loop
i = 2;
for t = times
%% Create the actual trajectory, using the velocities and
% accelerations in the inertial frame to compute the positions
[ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
currentPoseGlobal, omega, velocity, velocity, deltaT);
%% Simulate IMU measurements, considering Coriolis effect
% (in this simple example we neglect gravity and there are no other forces acting on the body)
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
omega, omega, velocity, velocity, deltaT);
%% Integrate in the body frame
[ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ...
currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity);
%% Integrate in the navigation frame
[ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ...
currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT);
%% Store data in some structure for statistics and plots
positions(:,i) = currentPoseGlobal.translation.vector;
positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector;
positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector;
% -
poses(i).p = positions(:,i);
posesIMUbody(i).p = positionsIMUbody(:,i);
posesIMUnav(i).p = positionsIMUnav(:,i);
% -
poses(i).R = currentPoseGlobal.rotation.matrix;
posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix;
posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix;
i = i + 1;
end
figure(1)
hold on;
plot(positions(1,:), positions(2,:), '-b');
plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r');
plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k');
axis equal;
legend('true trajectory', 'traj integrated in body', 'traj integrated in nav')

157
tests/testPCGSolver.cpp Normal file
View File

@ -0,0 +1,157 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file testPCGSolver.cpp
* @brief Unit tests for PCGSolver class
* @author Yong-Dian Jian
*/
#include <tests/smallExample.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/PCGSolver.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/SubgraphPreconditioner.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/Matrix.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <iostream>
#include <fstream>
using namespace std;
using namespace gtsam;
const double tol = 1e-3;
using symbol_shorthand::X;
using symbol_shorthand::L;
/* ************************************************************************* */
TEST( PCGSolver, llt ) {
Matrix R = (Matrix(3,3) <<
1., -1., -1.,
0., 2., -1.,
0., 0., 1.);
Matrix AtA = R.transpose() * R;
Vector Rvector = (Vector(9) << 1., -1., -1.,
0., 2., -1.,
0., 0., 1.);
// Vector Rvector = (Vector(6) << 1., -1., -1.,
// 2., -1.,
// 1.);
Vector b = (Vector(3) << 1., 2., 3.);
Vector x = (Vector(3) << 6.5, 2.5, 3.) ;
/* test cholesky */
Matrix Rhat = AtA.llt().matrixL().transpose();
EXPECT(assert_equal(R, Rhat, 1e-5));
/* test backward substitution */
Vector xhat = Rhat.triangularView<Eigen::Upper>().solve(b);
EXPECT(assert_equal(x, xhat, 1e-5));
/* test in-place back substitution */
xhat = b;
Rhat.triangularView<Eigen::Upper>().solveInPlace(xhat);
EXPECT(assert_equal(x, xhat, 1e-5));
/* test triangular matrix map */
Eigen::Map<Eigen::MatrixXd> Radapter(Rvector.data(), 3, 3);
xhat = Radapter.transpose().triangularView<Eigen::Upper>().solve(b);
EXPECT(assert_equal(x, xhat, 1e-5));
}
/* ************************************************************************* */
TEST( PCGSolver, dummy )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<DummyPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Values c0;
c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}
/* ************************************************************************* */
TEST( PCGSolver, blockjacobi )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<BlockJacobiPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Values c0;
c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}
/* ************************************************************************* */
TEST( PCGSolver, subgraph )
{
LevenbergMarquardtParams paramsPCG;
paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative;
PCGSolverParameters::shared_ptr pcg = boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ = boost::make_shared<SubgraphPreconditionerParameters>();
paramsPCG.iterativeParams = pcg;
NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph();
Point2 x0(10,10);
Values c0;
c0.insert(X(1), x0);
Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize();
DOUBLES_EQUAL(0,fg.error(actualPCG),tol);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}