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

Fix everything to work with no deprecated methods allowed.

release/4.3a0
Frank Dellaert 2022-01-02 16:19:17 -05:00
parent 6d0c55901c
commit 906176291f
7 changed files with 48 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/linear/GaussianConditional.cpp
View File

@ -193,6 +193,7 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const { void GaussianConditional::scaleFrontalsBySigma(VectorValues& gy) const {
DenseIndex vectorPosition = 0; DenseIndex vectorPosition = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) { for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
@ -200,5 +201,6 @@ namespace gtsam {
vectorPosition += getDim(frontal); vectorPosition += getDim(frontal);
} }
} }
#endif
} // namespace gtsam } // namespace gtsam

2
gtsam/linear/GaussianConditional.h
View File

@ -128,7 +128,7 @@ namespace gtsam {
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** Scale the values in \c gy according to the sigmas for the frontal variables in this /** Scale the values in \c gy according to the sigmas for the frontal variables in this
* conditional. */ * conditional. */
void GTSAM_DEPRECATED scaleFrontalsBySigma(VectorValues& gy) const; void scaleFrontalsBySigma(VectorValues& gy) const;
#endif #endif
private: private:

6
gtsam/linear/NoiseModel.h
View File

@ -730,16 +730,14 @@ namespace gtsam {
} // namespace noiseModel } // namespace noiseModel
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /**
/** Note, deliberately not in noiseModel namespace. * Aliases. Deliberately not in noiseModel namespace.
* Deprecated. Only for compatibility with previous version.
*/ */
typedef noiseModel::Base::shared_ptr SharedNoiseModel; typedef noiseModel::Base::shared_ptr SharedNoiseModel;
typedef noiseModel::Gaussian::shared_ptr SharedGaussian; typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
typedef noiseModel::Diagonal::shared_ptr SharedDiagonal; typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
typedef noiseModel::Constrained::shared_ptr SharedConstrained; typedef noiseModel::Constrained::shared_ptr SharedConstrained;
typedef noiseModel::Isotropic::shared_ptr SharedIsotropic; typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
#endif
/// traits /// traits
template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {}; template<> struct traits<noiseModel::Gaussian> : public Testable<noiseModel::Gaussian> {};

24
gtsam/navigation/AHRSFactor.cpp
View File

@ -168,13 +168,12 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Rot3 AHRSFactor::Predict( Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias,
const Rot3& rot_i, const Vector3& bias, const PreintegratedAhrsMeasurements& pim) {
const PreintegratedAhrsMeasurements preintegratedMeasurements) { const Vector3 biascorrectedOmega = pim.predict(bias);
const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
// Coriolis term // Coriolis term
const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i); const Vector3 coriolis = pim.integrateCoriolis(rot_i);
const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Vector3 correctedOmega = biascorrectedOmega - coriolis;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega); const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
@ -184,27 +183,26 @@ Rot3 AHRSFactor::Predict(
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias, AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& pim, const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor) const boost::optional<Pose3>& body_P_sensor)
: Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j, bias), : Base(noiseModel::Gaussian::Covariance(pim.preintMeasCov_), rot_i, rot_j,
bias),
_PIM_(pim) { _PIM_(pim) {
boost::shared_ptr<PreintegratedMeasurements::Params> p = auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->body_P_sensor = body_P_sensor; p->body_P_sensor = body_P_sensor;
_PIM_.p_ = p; _PIM_.p_ = p;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias, Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements pim, const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor) { const boost::optional<Pose3>& body_P_sensor) {
boost::shared_ptr<PreintegratedMeasurements::Params> p = auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
p->omegaCoriolis = omegaCoriolis; p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor; p->body_P_sensor = body_P_sensor;
PreintegratedMeasurements newPim = pim; PreintegratedAhrsMeasurements newPim = pim;
newPim.p_ = p; newPim.p_ = p;
return Predict(rot_i, bias, newPim); return Predict(rot_i, bias, newPim);
} }

36
gtsam/navigation/AHRSFactor.h
View File

@ -104,15 +104,13 @@ class GTSAM_EXPORT PreintegratedAhrsMeasurements : public PreintegratedRotation
static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt, static Vector DeltaAngles(const Vector& msr_gyro_t, const double msr_dt,
const Vector3& delta_angles); const Vector3& delta_angles);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 /// @deprecated constructor, but used in tests.
/// @deprecated constructor PreintegratedAhrsMeasurements(const Vector3& biasHat,
GTSAM_DEPRECATED PreintegratedAhrsMeasurements( const Matrix3& measuredOmegaCovariance)
const Vector3& biasHat, const Matrix3& measuredOmegaCovariance)
: PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) { : PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) {
p_->gyroscopeCovariance = measuredOmegaCovariance; p_->gyroscopeCovariance = measuredOmegaCovariance;
resetIntegration(); resetIntegration();
} }
#endif
private: private:
@ -183,27 +181,25 @@ public:
/// predicted states from IMU /// predicted states from IMU
/// TODO(frank): relationship with PIM predict ?? /// TODO(frank): relationship with PIM predict ??
static Rot3 Predict( static Rot3 Predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim);
/// @deprecated constructor, but used in tests.
AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
/// @deprecated static function, but used in tests.
static Rot3 predict(
const Rot3& rot_i, const Vector3& bias, const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements preintegratedMeasurements); const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @deprecated name /// @deprecated name
typedef PreintegratedAhrsMeasurements PreintegratedMeasurements; typedef PreintegratedAhrsMeasurements PreintegratedMeasurements;
/// @deprecated constructor
GTSAM_DEPRECATED AHRSFactor(
Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& preintegratedMeasurements,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
/// @deprecated static function
static Rot3 GTSAM_DEPRECATED
predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
#endif #endif
private: private:

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

@ -54,11 +54,11 @@ Rot3 evaluateRotationError(const AHRSFactor& factor, const Rot3 rot_i,
return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3)); return Rot3::Expmap(factor.evaluateError(rot_i, rot_j, bias).tail(3));
} }
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements( PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
const Vector3& bias, const list<Vector3>& measuredOmegas, const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs, const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3::Zero()) { const Vector3& initialRotationRate = Vector3::Zero()) {
AHRSFactor::PreintegratedMeasurements result(bias, I_3x3); PreintegratedAhrsMeasurements result(bias, I_3x3);
list<Vector3>::const_iterator itOmega = measuredOmegas.begin(); list<Vector3>::const_iterator itOmega = measuredOmegas.begin();
list<double>::const_iterator itDeltaT = deltaTs.begin(); list<double>::const_iterator itDeltaT = deltaTs.begin();
@ -86,10 +86,10 @@ Rot3 evaluateRotation(const Vector3 measuredOmega, const Vector3 biasOmega,
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) { Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta) {
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta))); return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
} }
} }
//****************************************************************************** //******************************************************************************
TEST( AHRSFactor, PreintegratedMeasurements ) { TEST( AHRSFactor, PreintegratedAhrsMeasurements ) {
// Linearization point // Linearization point
Vector3 bias(0,0,0); ///< Current estimate of angular rate bias Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
@ -102,7 +102,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
double expectedDeltaT1(0.5); double expectedDeltaT1(0.5);
// Actual preintegrated values // Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3); PreintegratedAhrsMeasurements actual1(bias, Z_3x3);
actual1.integrateMeasurement(measuredOmega, deltaT); actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
@ -113,7 +113,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
double expectedDeltaT2(1); double expectedDeltaT2(1);
// Actual preintegrated values // Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual2 = actual1; PreintegratedAhrsMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredOmega, deltaT); actual2.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6)); EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
@ -159,7 +159,7 @@ TEST(AHRSFactor, Error) {
Vector3 measuredOmega; Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0; measuredOmega << M_PI / 100, 0, 0;
double deltaT = 1.0; double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3); PreintegratedAhrsMeasurements pim(bias, Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT); pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor // Create factor
@ -217,7 +217,7 @@ TEST(AHRSFactor, ErrorWithBiases) {
measuredOmega << 0, 0, M_PI / 10.0 + 0.3; measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
double deltaT = 1.0; double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0), PreintegratedAhrsMeasurements pim(Vector3(0,0,0),
Z_3x3); Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT); pim.integrateMeasurement(measuredOmega, deltaT);
@ -360,7 +360,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
} }
// Actual preintegrated values // Actual preintegrated values
AHRSFactor::PreintegratedMeasurements preintegrated = PreintegratedAhrsMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs, evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
Vector3(M_PI / 100.0, 0.0, 0.0)); Vector3(M_PI / 100.0, 0.0, 0.0));
@ -397,7 +397,7 @@ TEST( AHRSFactor, ErrorWithBiasesAndSensorBodyDisplacement ) {
const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)), const Pose3 body_P_sensor(Rot3::Expmap(Vector3(0, 0.10, 0.10)),
Point3(1, 0, 0)); Point3(1, 0, 0));
AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance); PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
pim.integrateMeasurement(measuredOmega, deltaT); pim.integrateMeasurement(measuredOmega, deltaT);
@ -439,7 +439,7 @@ TEST (AHRSFactor, predictTest) {
Vector3 measuredOmega; Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0; measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.2; double deltaT = 0.2;
AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance); PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance);
for (int i = 0; i < 1000; ++i) { for (int i = 0; i < 1000; ++i) {
pim.integrateMeasurement(measuredOmega, deltaT); pim.integrateMeasurement(measuredOmega, deltaT);
} }
@ -456,9 +456,9 @@ TEST (AHRSFactor, predictTest) {
Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis); Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6)); EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
// AHRSFactor::PreintegratedMeasurements::predict // PreintegratedAhrsMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>( Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
std::bind(&AHRSFactor::PreintegratedMeasurements::predict, std::bind(&PreintegratedAhrsMeasurements::predict,
&pim, std::placeholders::_1, boost::none), bias); &pim, std::placeholders::_1, boost::none), bias);
// Actual Jacobians // Actual Jacobians
@ -478,7 +478,7 @@ TEST (AHRSFactor, graphTest) {
// PreIntegrator // PreIntegrator
Vector3 biasHat(0, 0, 0); Vector3 biasHat(0, 0, 0);
AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance); PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance);
// Pre-integrate measurements // Pre-integrate measurements
Vector3 measuredOmega(0, M_PI / 20, 0); Vector3 measuredOmega(0, M_PI / 20, 0);

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

@ -47,20 +47,19 @@ TEST(ImuBias, Constructor) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
TEST(ImuBias, inverse) { TEST(ImuBias, inverse) {
Bias biasActual = bias1.inverse(); Bias biasActual = bias1.inverse();
Bias biasExpected = Bias(-biasAcc1, -biasGyro1); Bias biasExpected = Bias(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */
TEST(ImuBias, compose) { TEST(ImuBias, compose) {
Bias biasActual = bias2.compose(bias1); Bias biasActual = bias2.compose(bias1);
Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2); Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */
TEST(ImuBias, between) { TEST(ImuBias, between) {
// p.between(q) == q - p // p.between(q) == q - p
Bias biasActual = bias2.between(bias1); Bias biasActual = bias2.between(bias1);
@ -68,7 +67,6 @@ TEST(ImuBias, between) {
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */
TEST(ImuBias, localCoordinates) { TEST(ImuBias, localCoordinates) {
Vector deltaActual = Vector(bias2.localCoordinates(bias1)); Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected = Vector deltaExpected =
@ -76,7 +74,6 @@ TEST(ImuBias, localCoordinates) {
EXPECT(assert_equal(deltaExpected, deltaActual)); EXPECT(assert_equal(deltaExpected, deltaActual));
} }
/* ************************************************************************* */
TEST(ImuBias, retract) { TEST(ImuBias, retract) {
Vector6 delta; Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
@ -86,14 +83,12 @@ TEST(ImuBias, retract) {
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
/* ************************************************************************* */
TEST(ImuBias, Logmap) { TEST(ImuBias, Logmap) {
Vector deltaActual = bias2.Logmap(bias1); Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector(); Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual)); EXPECT(assert_equal(deltaExpected, deltaActual));
} }
/* ************************************************************************* */
TEST(ImuBias, Expmap) { TEST(ImuBias, Expmap) {
Vector6 delta; Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2; delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
@ -101,6 +96,7 @@ TEST(ImuBias, Expmap) {
Bias biasExpected = Bias(delta); Bias biasExpected = Bias(delta);
EXPECT(assert_equal(biasExpected, biasActual)); EXPECT(assert_equal(biasExpected, biasActual));
} }
#endif
/* ************************************************************************* */ /* ************************************************************************* */
TEST(ImuBias, operatorSub) { TEST(ImuBias, operatorSub) {