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 {
DenseIndex vectorPosition = 0;
for (const_iterator frontal = beginFrontals(); frontal != endFrontals(); ++frontal) {
@ -200,5 +201,6 @@ namespace gtsam {
vectorPosition += getDim(frontal);
}
}
#endif
} // namespace gtsam

2
gtsam/linear/GaussianConditional.h
View File

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

6
gtsam/linear/NoiseModel.h
View File

@ -730,16 +730,14 @@ namespace gtsam {
} // namespace noiseModel
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/** Note, deliberately not in noiseModel namespace.
* Deprecated. Only for compatibility with previous version.
/**
* Aliases. Deliberately not in noiseModel namespace.
*/
typedef noiseModel::Base::shared_ptr SharedNoiseModel;
typedef noiseModel::Gaussian::shared_ptr SharedGaussian;
typedef noiseModel::Diagonal::shared_ptr SharedDiagonal;
typedef noiseModel::Constrained::shared_ptr SharedConstrained;
typedef noiseModel::Isotropic::shared_ptr SharedIsotropic;
#endif
/// traits
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(
const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements preintegratedMeasurements) {
const Vector3 biascorrectedOmega = preintegratedMeasurements.predict(bias);
Rot3 AHRSFactor::Predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedAhrsMeasurements& pim) {
const Vector3 biascorrectedOmega = pim.predict(bias);
// Coriolis term
const Vector3 coriolis = preintegratedMeasurements.integrateCoriolis(rot_i);
const Vector3 coriolis = pim.integrateCoriolis(rot_i);
const Vector3 correctedOmega = biascorrectedOmega - coriolis;
const Rot3 correctedDeltaRij = Rot3::Expmap(correctedOmega);
@ -184,27 +183,26 @@ Rot3 AHRSFactor::Predict(
//------------------------------------------------------------------------------
AHRSFactor::AHRSFactor(Key rot_i, Key rot_j, Key bias,
const PreintegratedMeasurements& pim,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
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) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
p->body_P_sensor = body_P_sensor;
_PIM_.p_ = p;
}
//------------------------------------------------------------------------------
Rot3 AHRSFactor::predict(const Rot3& rot_i, const Vector3& bias,
const PreintegratedMeasurements pim,
const PreintegratedAhrsMeasurements& pim,
const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor) {
boost::shared_ptr<PreintegratedMeasurements::Params> p =
boost::make_shared<PreintegratedMeasurements::Params>(pim.p());
auto p = boost::make_shared<PreintegratedAhrsMeasurements::Params>(pim.p());
p->omegaCoriolis = omegaCoriolis;
p->body_P_sensor = body_P_sensor;
PreintegratedMeasurements newPim = pim;
PreintegratedAhrsMeasurements newPim = pim;
newPim.p_ = p;
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,
const Vector3& delta_angles);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @deprecated constructor
GTSAM_DEPRECATED PreintegratedAhrsMeasurements(
const Vector3& biasHat, const Matrix3& measuredOmegaCovariance)
/// @deprecated constructor, but used in tests.
PreintegratedAhrsMeasurements(const Vector3& biasHat,
const Matrix3& measuredOmegaCovariance)
: PreintegratedRotation(boost::make_shared<Params>()), biasHat_(biasHat) {
p_->gyroscopeCovariance = measuredOmegaCovariance;
resetIntegration();
}
#endif
private:
@ -183,27 +181,25 @@ public:
/// predicted states from IMU
/// 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 PreintegratedAhrsMeasurements preintegratedMeasurements);
const PreintegratedAhrsMeasurements& pim, const Vector3& omegaCoriolis,
const boost::optional<Pose3>& body_P_sensor = boost::none);
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V42
/// @deprecated name
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
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));
}
AHRSFactor::PreintegratedMeasurements evaluatePreintegratedMeasurements(
PreintegratedAhrsMeasurements evaluatePreintegratedMeasurements(
const Vector3& bias, const list<Vector3>& measuredOmegas,
const list<double>& deltaTs,
const Vector3& initialRotationRate = Vector3::Zero()) {
AHRSFactor::PreintegratedMeasurements result(bias, I_3x3);
PreintegratedAhrsMeasurements result(bias, I_3x3);
list<Vector3>::const_iterator itOmega = measuredOmegas.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) {
return Rot3::Logmap(Rot3::Expmap(thetahat).compose(Rot3::Expmap(deltatheta)));
}
}
//******************************************************************************
TEST( AHRSFactor, PreintegratedMeasurements ) {
TEST( AHRSFactor, PreintegratedAhrsMeasurements ) {
// Linearization point
Vector3 bias(0,0,0); ///< Current estimate of angular rate bias
@ -102,7 +102,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
double expectedDeltaT1(0.5);
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual1(bias, Z_3x3);
PreintegratedAhrsMeasurements actual1(bias, Z_3x3);
actual1.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR1, Rot3(actual1.deltaRij()), 1e-6));
@ -113,7 +113,7 @@ TEST( AHRSFactor, PreintegratedMeasurements ) {
double expectedDeltaT2(1);
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements actual2 = actual1;
PreintegratedAhrsMeasurements actual2 = actual1;
actual2.integrateMeasurement(measuredOmega, deltaT);
EXPECT(assert_equal(expectedDeltaR2, Rot3(actual2.deltaRij()), 1e-6));
@ -159,7 +159,7 @@ TEST(AHRSFactor, Error) {
Vector3 measuredOmega;
measuredOmega << M_PI / 100, 0, 0;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pim(bias, Z_3x3);
PreintegratedAhrsMeasurements pim(bias, Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT);
// Create factor
@ -217,7 +217,7 @@ TEST(AHRSFactor, ErrorWithBiases) {
measuredOmega << 0, 0, M_PI / 10.0 + 0.3;
double deltaT = 1.0;
AHRSFactor::PreintegratedMeasurements pim(Vector3(0,0,0),
PreintegratedAhrsMeasurements pim(Vector3(0,0,0),
Z_3x3);
pim.integrateMeasurement(measuredOmega, deltaT);
@ -360,7 +360,7 @@ TEST( AHRSFactor, FirstOrderPreIntegratedMeasurements ) {
}
// Actual preintegrated values
AHRSFactor::PreintegratedMeasurements preintegrated =
PreintegratedAhrsMeasurements preintegrated =
evaluatePreintegratedMeasurements(bias, measuredOmegas, deltaTs,
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)),
Point3(1, 0, 0));
AHRSFactor::PreintegratedMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(Vector3::Zero(), kMeasuredAccCovariance);
pim.integrateMeasurement(measuredOmega, deltaT);
@ -439,7 +439,7 @@ TEST (AHRSFactor, predictTest) {
Vector3 measuredOmega;
measuredOmega << 0, 0, M_PI / 10.0;
double deltaT = 0.2;
AHRSFactor::PreintegratedMeasurements pim(bias, kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(bias, kMeasuredAccCovariance);
for (int i = 0; i < 1000; ++i) {
pim.integrateMeasurement(measuredOmega, deltaT);
}
@ -456,9 +456,9 @@ TEST (AHRSFactor, predictTest) {
Rot3 actualRot = factor.predict(x, bias, pim, kZeroOmegaCoriolis);
EXPECT(assert_equal(expectedRot, actualRot, 1e-6));
// AHRSFactor::PreintegratedMeasurements::predict
// PreintegratedAhrsMeasurements::predict
Matrix expectedH = numericalDerivative11<Vector3, Vector3>(
std::bind(&AHRSFactor::PreintegratedMeasurements::predict,
std::bind(&PreintegratedAhrsMeasurements::predict,
&pim, std::placeholders::_1, boost::none), bias);
// Actual Jacobians
@ -478,7 +478,7 @@ TEST (AHRSFactor, graphTest) {
// PreIntegrator
Vector3 biasHat(0, 0, 0);
AHRSFactor::PreintegratedMeasurements pim(biasHat, kMeasuredAccCovariance);
PreintegratedAhrsMeasurements pim(biasHat, kMeasuredAccCovariance);
// Pre-integrate measurements
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) {
Bias biasActual = bias1.inverse();
Bias biasExpected = Bias(-biasAcc1, -biasGyro1);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, compose) {
Bias biasActual = bias2.compose(bias1);
Bias biasExpected = Bias(biasAcc1 + biasAcc2, biasGyro1 + biasGyro2);
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, between) {
// p.between(q) == q - p
Bias biasActual = bias2.between(bias1);
@ -68,7 +67,6 @@ TEST(ImuBias, between) {
EXPECT(assert_equal(biasExpected, biasActual));
}
/* ************************************************************************* */
TEST(ImuBias, localCoordinates) {
Vector deltaActual = Vector(bias2.localCoordinates(bias1));
Vector deltaExpected =
@ -76,7 +74,6 @@ TEST(ImuBias, localCoordinates) {
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST(ImuBias, retract) {
Vector6 delta;
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));
}
/* ************************************************************************* */
TEST(ImuBias, Logmap) {
Vector deltaActual = bias2.Logmap(bias1);
Vector deltaExpected = bias1.vector();
EXPECT(assert_equal(deltaExpected, deltaActual));
}
/* ************************************************************************* */
TEST(ImuBias, Expmap) {
Vector6 delta;
delta << 0.1, 0.2, -0.3, 0.1, -0.1, 0.2;
@ -101,6 +96,7 @@ TEST(ImuBias, Expmap) {
Bias biasExpected = Bias(delta);
EXPECT(assert_equal(biasExpected, biasActual));
}
#endif
/* ************************************************************************* */
TEST(ImuBias, operatorSub) {