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

Moved ImuFactor and CombinedImuFactor into main gtsam, and moved InertialNavFactor family into gtsam_unstable.

release/4.3a0
Richard Roberts 2013-08-13 21:04:31 +00:00
parent a2d00e73e1
commit 16b4f0fe8b
12 changed files with 71 additions and 74 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

65
gtsam.h
View File

@ -2160,6 +2160,71 @@ virtual class ConstantBias : gtsam::Value {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
};
virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
};
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);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
//*************************************************************************
// Utilities
//*************************************************************************

0
gtsam_unstable/slam/CombinedImuFactor.h → gtsam/navigation/CombinedImuFactor.h
View File

0
gtsam_unstable/slam/ImuFactor.h → gtsam/navigation/ImuFactor.h
View File

5
gtsam_unstable/slam/tests/testCombinedImuFactor.cpp → gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -15,11 +15,10 @@
* @author Luca Carlone, Stephen Williams, Richard Roberts
*/
#include <gtsam_unstable/slam/ImuFactor.h>
#include <gtsam_unstable/slam/CombinedImuFactor.h>
#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>

3
gtsam_unstable/slam/tests/testImuFactor.cpp → gtsam/navigation/tests/testImuFactor.cpp
View File

@ -15,8 +15,7 @@
* @author Luca Carlone, Stephen Williams, Richard Roberts
*/
#include <gtsam_unstable/slam/ImuFactor.h>
#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/navigation/ImuBias.h>

66
gtsam_unstable/gtsam_unstable.h
View File

@ -629,72 +629,6 @@ virtual class InvDepthFactorVariant3b : gtsam::NonlinearFactor {
InvDepthFactorVariant3b(size_t poseKey1, size_t poseKey2, size_t landmarkKey, const gtsam::Point2& measured, const gtsam::Cal3_S2* K, const gtsam::noiseModel::Base* model);
};
#include <gtsam_unstable/slam/ImuFactor.h>
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
};
virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam_unstable/slam/CombinedImuFactor.h>
class CombinedImuFactorPreintegratedMeasurements {
// Standard Constructor
CombinedImuFactorPreintegratedMeasurements(
const gtsam::imuBias::ConstantBias& bias,
Matrix measuredAccCovariance,
Matrix measuredOmegaCovariance,
Matrix integrationErrorCovariance,
Matrix biasAccCovariance,
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
};
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);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam_unstable/slam/Mechanization_bRn2.h>
class Mechanization_bRn2 {

0
gtsam/navigation/EquivInertialNavFactor_GlobalVel.h → gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h
View File

0
gtsam/navigation/EquivInertialNavFactor_GlobalVel_NoBias.h → gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel_NoBias.h
View File

0
gtsam/navigation/InertialNavFactor_GlobalVelocity.h → gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h
View File

2
gtsam/navigation/tests/testEquivInertialNavFactor_GlobalVel.cpp → gtsam_unstable/slam/tests/testEquivInertialNavFactor_GlobalVel.cpp
View File

@ -15,7 +15,7 @@
* @author Vadim Indelman, Stephen Williams
*/
#include <gtsam/navigation/EquivInertialNavFactor_GlobalVel.h>
#include <gtsam_unstable/slam/EquivInertialNavFactor_GlobalVel.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>

2
gtsam/navigation/tests/testInertialNavFactor_GlobalVelocity.cpp → gtsam_unstable/slam/tests/testInertialNavFactor_GlobalVelocity.cpp
View File

@ -18,7 +18,7 @@
#include <iostream>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/InertialNavFactor_GlobalVelocity.h>
#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>

2
gtsam/navigation/tests/timeInertialNavFactor_GlobalVelocity.cpp → gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp
View File

@ -17,7 +17,7 @@
#include <iostream>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/navigation/InertialNavFactor_GlobalVelocity.h>
#include <gtsam_unstable/slam/InertialNavFactor_GlobalVelocity.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Key.h>