55 lines
1.9 KiB
C++
55 lines
1.9 KiB
C++
/* ----------------------------------------------------------------------------
|
|
|
|
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
|
* Atlanta, Georgia 30332-0415
|
|
* All Rights Reserved
|
|
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
|
|
|
* See LICENSE for the license information
|
|
|
|
* -------------------------------------------------------------------------- */
|
|
|
|
/**
|
|
* @file PreintegrationParams.h
|
|
* @author Luca Carlone
|
|
* @author Stephen Williams
|
|
* @author Richard Roberts
|
|
* @author Vadim Indelman
|
|
* @author David Jensen
|
|
* @author Frank Dellaert
|
|
**/
|
|
|
|
#include "PreintegrationParams.h"
|
|
|
|
using namespace std;
|
|
|
|
namespace gtsam {
|
|
|
|
//------------------------------------------------------------------------------
|
|
void PreintegrationParams::print(const string& s) const {
|
|
PreintegratedRotationParams::print(s);
|
|
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
|
|
<< endl;
|
|
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
|
|
<< endl;
|
|
if (omegaCoriolis && use2ndOrderCoriolis)
|
|
cout << "Using 2nd-order Coriolis" << endl;
|
|
if (body_P_sensor) body_P_sensor->print(" ");
|
|
cout << "n_gravity = (" << n_gravity.transpose() << ")" << endl;
|
|
}
|
|
|
|
//------------------------------------------------------------------------------
|
|
bool PreintegrationParams::equals(const PreintegratedRotationParams& other,
|
|
double tol) const {
|
|
auto e = dynamic_cast<const PreintegrationParams*>(&other);
|
|
return e != nullptr && PreintegratedRotationParams::equals(other, tol) &&
|
|
use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
|
|
equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
|
|
tol) &&
|
|
equal_with_abs_tol(integrationCovariance, e->integrationCovariance,
|
|
tol) &&
|
|
equal_with_abs_tol(n_gravity, e->n_gravity, tol);
|
|
}
|
|
|
|
} // namespace gtsam
|