Tying up loose ends, ostream, get rid of cov_
Frank Dellaert
parent
3a891b2a2c
commit
15dfd932f1
|
|
@ -32,7 +32,6 @@ namespace gtsam {
|
|||
PreintegrationBase::PreintegrationBase(const boost::shared_ptr<Params>& p,
|
||||
const Bias& biasHat)
|
||||
: p_(p), biasHat_(biasHat), deltaTij_(0.0) {
|
||||
cov_.setZero();
|
||||
resetIntegration();
|
||||
}
|
||||
|
||||
|
|
@ -47,14 +46,20 @@ void PreintegrationBase::resetIntegration() {
|
|||
delVdelBiasOmega_ = Z_3x3;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
ostream& operator<<(ostream& os, const PreintegrationBase& pim) {
|
||||
os << " deltaTij " << pim.deltaTij_ << endl;
|
||||
os << " deltaRij " << Point3(pim.theta()) << endl;
|
||||
os << " deltaPij " << Point3(pim.deltaPij()) << endl;
|
||||
os << " deltaVij " << Point3(pim.deltaVij()) << endl;
|
||||
os << " gyrobias " << Point3(pim.biasHat_.gyroscope()) << endl;
|
||||
os << " acc_bias " << Point3(pim.biasHat_.accelerometer()) << endl;
|
||||
return os;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegrationBase::print(const string& s) const {
|
||||
cout << s << endl;
|
||||
cout << " deltaTij [" << deltaTij_ << "]" << endl;
|
||||
cout << " deltaRij.ypr = (" << deltaRij().ypr().transpose() << ")" << endl;
|
||||
cout << " deltaPij [ " << deltaPij().transpose() << " ]" << endl;
|
||||
cout << " deltaVij [ " << deltaVij().transpose() << " ]" << endl;
|
||||
biasHat_.print(" biasHat");
|
||||
cout << s << *this << endl;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
@ -297,27 +302,6 @@ NavState PreintegrationBase::predict(const NavState& state_i,
|
|||
return state_j;
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
SharedGaussian PreintegrationBase::noiseModel() const {
|
||||
// Correct for application of retract, by calculating the retract derivative H
|
||||
// From NavState::retract:
|
||||
Matrix3 D_R_theta;
|
||||
const Matrix3 iRj = Rot3::Expmap(theta(), D_R_theta).matrix();
|
||||
Matrix9 H;
|
||||
H << D_R_theta, Z_3x3, Z_3x3, //
|
||||
Z_3x3, iRj.transpose(), Z_3x3, //
|
||||
Z_3x3, Z_3x3, iRj.transpose();
|
||||
|
||||
// TODO(frank): theta() itself is noisy, so should we not correct for that?
|
||||
Matrix9 HcH = H * cov_ * H.transpose();
|
||||
return noiseModel::Gaussian::Covariance(HcH, false);
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Matrix9 PreintegrationBase::preintMeasCov() const {
|
||||
return noiseModel()->covariance();
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
Vector9 PreintegrationBase::computeErrorAndJacobians(const Pose3& pose_i,
|
||||
const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
|
||||
|
|
|
|||
|
|
@ -26,6 +26,8 @@
|
|||
#include <gtsam/navigation/ImuBias.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
|
||||
#include <iosfwd>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
#define ALLOW_DEPRECATED_IN_GTSAM4
|
||||
|
|
@ -119,7 +121,6 @@ class PreintegrationBase {
|
|||
*/
|
||||
/// Current estimate of deltaXij_k
|
||||
TangentVector deltaXij_;
|
||||
Matrix9 cov_;
|
||||
|
||||
Matrix3 delRdelBiasOmega_; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias
|
||||
Matrix3 delPdelBiasAcc_; ///< Jacobian of preintegrated position w.r.t. acceleration bias
|
||||
|
|
@ -194,7 +195,6 @@ public:
|
|||
const double& deltaTij() const { return deltaTij_; }
|
||||
|
||||
const Vector9& zeta() const { return deltaXij_.vector(); }
|
||||
const Matrix9& zetaCov() const { return cov_; }
|
||||
|
||||
Vector3 theta() const { return deltaXij_.theta(); }
|
||||
Vector3 deltaPij() const { return deltaXij_.position(); }
|
||||
|
|
@ -215,6 +215,7 @@ public:
|
|||
|
||||
/// @name Testable
|
||||
/// @{
|
||||
GTSAM_EXPORT friend std::ostream& operator<<(std::ostream& os, const PreintegrationBase& pim);
|
||||
void print(const std::string& s) const;
|
||||
bool equals(const PreintegrationBase& other, double tol) const;
|
||||
/// @}
|
||||
|
|
@ -264,12 +265,6 @@ public:
|
|||
OptionalJacobian<9, 9> H1 = boost::none,
|
||||
OptionalJacobian<9, 6> H2 = boost::none) const;
|
||||
|
||||
/// Return Gaussian noise model on prediction
|
||||
SharedGaussian noiseModel() const;
|
||||
|
||||
/// @deprecated: Explicitly calculate covariance
|
||||
Matrix9 preintMeasCov() const;
|
||||
|
||||
/// Compute errors w.r.t. preintegrated measurements and jacobians wrt pose_i, vel_i, bias_i, pose_j, bias_j
|
||||
Vector9 computeErrorAndJacobians(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const Pose3& pose_j, const Vector3& vel_j,
|
||||
|
|
|
|||
|
|
@ -33,9 +33,9 @@ static const double kAccelSigma = 0.1 / 60.0;
|
|||
static const Vector3 kAccBias(0.2, 0, 0), kRotBias(0.1, 0, 0.3);
|
||||
static const imuBias::ConstantBias kNonZeroBias(kAccBias, kRotBias);
|
||||
|
||||
// Create default parameters with Z-down and above noise parameters
|
||||
// Create default parameters with Z-up and above noise parameters
|
||||
static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
|
||||
auto p = PreintegrationParams::MakeSharedD(10);
|
||||
auto p = PreintegrationParams::MakeSharedU(10);
|
||||
p->gyroscopeCovariance = kGyroSigma * kGyroSigma * I_3x3;
|
||||
p->accelerometerCovariance = kAccelSigma * kAccelSigma * I_3x3;
|
||||
p->integrationCovariance = 0.0000001 * I_3x3;
|
||||
|
|
@ -46,7 +46,7 @@ static boost::shared_ptr<PreintegratedImuMeasurements::Params> defaultParams() {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Spin) {
|
||||
// angular velocity 6 kDegree/sec
|
||||
// angular velocity 6 degree/sec
|
||||
const double w = 6 * kDegree;
|
||||
const Vector3 W(0, 0, w), V(0, 0, 0);
|
||||
const ConstantTwistScenario scenario(W, V);
|
||||
|
|
@ -104,7 +104,7 @@ TEST(ScenarioRunner, ForwardWithBias) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Circle) {
|
||||
// Forward velocity 2m/s, angular velocity 6 kDegree/sec
|
||||
// Forward velocity 2m/s, angular velocity 6 degree/sec
|
||||
const double v = 2, w = 6 * kDegree;
|
||||
ConstantTwistScenario scenario(Vector3(0, 0, w), Vector3(v, 0, 0));
|
||||
|
||||
|
|
@ -122,7 +122,7 @@ TEST(ScenarioRunner, Circle) {
|
|||
/* ************************************************************************* */
|
||||
TEST(ScenarioRunner, Loop) {
|
||||
// Forward velocity 2m/s
|
||||
// Pitch up with angular velocity 6 kDegree/sec (negative in FLU)
|
||||
// Pitch up with angular velocity 6 degree/sec (negative in FLU)
|
||||
const double v = 2, w = 6 * kDegree;
|
||||
ConstantTwistScenario scenario(Vector3(0, -w, 0), Vector3(v, 0, 0));
|
||||
|
||||
|
|
|
|||
|
|
@ -70,13 +70,14 @@ void exportScenario() {
|
|||
class_<PreintegratedImuMeasurements>(
|
||||
"PreintegratedImuMeasurements",
|
||||
init<const boost::shared_ptr<PreintegrationParams>&,
|
||||
const imuBias::ConstantBias&>());
|
||||
const imuBias::ConstantBias&>()).def(repr(self));
|
||||
|
||||
class_<NavState>("NavState", init<>())
|
||||
// TODO(frank): overload with jacobians
|
||||
// .def("attitude", &NavState::attitude)
|
||||
// .def("position", &NavState::position)
|
||||
// .def("velocity", &NavState::velocity)
|
||||
.def(repr(self))
|
||||
.def("pose", &NavState::pose);
|
||||
|
||||
class_<imuBias::ConstantBias>("ConstantBias", init<>());
|
||||
|
|
|
|||
Loading…
Reference in New Issue