-
Notifications
You must be signed in to change notification settings - Fork 756
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Added non-default const. for AHRS pre-int. and params #93
Changes from 1 commit
38e43ce
04b9bf2
56cc2b0
162c9ab
3e83f00
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -35,6 +35,13 @@ struct GTSAM_EXPORT PreintegratedRotationParams { | |
|
||
PreintegratedRotationParams() : gyroscopeCovariance(I_3x3) {} | ||
|
||
PreintegratedRotationParams(Matrix3 gyroscope_covariance, | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more.
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Done |
||
boost::optional<Vector3> omega_coriolis) | ||
: gyroscopeCovariance(gyroscope_covariance) { | ||
if (omega_coriolis) | ||
omegaCoriolis.reset(omega_coriolis.get()); | ||
} | ||
|
||
virtual ~PreintegratedRotationParams() {} | ||
|
||
virtual void print(const std::string& s) const; | ||
|
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -119,6 +119,35 @@ TEST( AHRSFactor, PreintegratedMeasurements ) { | |
DOUBLES_EQUAL(expectedDeltaT2, actual2.deltaTij(), 1e-6); | ||
} | ||
|
||
//****************************************************************************** | ||
TEST( AHRSFactor, PreintegratedAhrsMeasurementsConstructor ) { | ||
// Linearization point | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. obsolete comment? There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Removed, got copied from above test I think. |
||
Matrix3 gyroscopeCovariance = Matrix3::Ones()*0.4; | ||
Vector3 omegaCoriolis(0.1, 0.5, 0.9); | ||
PreintegratedRotationParams params(gyroscopeCovariance, omegaCoriolis); | ||
Vector3 bias(1.0,2.0,3.0); ///< Current estimate of angular rate bias | ||
Rot3 deltaRij(Rot3::RzRyRx(M_PI / 12.0, M_PI / 6.0, M_PI / 4.0)); | ||
double deltaTij = 0.02; | ||
Matrix3 delRdelBiasOmega = Matrix3::Ones()*0.5; | ||
Matrix3 preintMeasCov = Matrix3::Ones()*0.2; | ||
gtsam::PreintegratedAhrsMeasurements test_pim( | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. using There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Done There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. test_pim -> There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. changed to |
||
boost::make_shared<gtsam::PreintegratedRotationParams>(params), | ||
bias, | ||
deltaTij, | ||
deltaRij, | ||
delRdelBiasOmega, | ||
preintMeasCov); | ||
EXPECT(assert_equal(gyroscopeCovariance, | ||
test_pim.p().getGyroscopeCovariance(), 1e-6)); | ||
EXPECT(assert_equal(omegaCoriolis, | ||
test_pim.p().getOmegaCoriolis().get(), 1e-6)); | ||
EXPECT(assert_equal(bias, test_pim.biasHat(), 1e-6)); | ||
DOUBLES_EQUAL(deltaTij, test_pim.deltaTij(), 1e-6); | ||
EXPECT(assert_equal(deltaRij, Rot3(test_pim.deltaRij()), 1e-6)); | ||
EXPECT(assert_equal(delRdelBiasOmega, test_pim.delRdelBiasOmega(), 1e-6)); | ||
EXPECT(assert_equal(preintMeasCov, test_pim.preintMeasCov(), 1e-6)); | ||
} | ||
|
||
/* ************************************************************************* */ | ||
TEST(AHRSFactor, Error) { | ||
// Linearization point | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why ?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Redundant indeed, base class already copies params as
p_(p)