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

Replace scoped name with direct name and instantiate base class in constructor

release/4.3a0
Varun Agrawal 2020-07-08 23:37:32 -04:00
parent 8d921c82a0
commit cc2456678f
3 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/navigation/PreintegratedRotation.cpp
View File

@ -25,7 +25,7 @@ using namespace std;
namespace gtsam { namespace gtsam {
void PreintegratedRotation::Params::print(const string& s) const { void PreintegratedRotationParams::print(const string& s) const {
cout << s << endl; cout << s << endl;
cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl; cout << "gyroscopeCovariance:\n[\n" << gyroscopeCovariance << "\n]" << endl;
if (omegaCoriolis) if (omegaCoriolis)
@ -34,8 +34,8 @@ void PreintegratedRotation::Params::print(const string& s) const {
body_P_sensor->print("body_P_sensor"); body_P_sensor->print("body_P_sensor");
} }
bool PreintegratedRotation::Params::equals( bool PreintegratedRotationParams::equals(
const PreintegratedRotation::Params& other, double tol) const { const PreintegratedRotationParams& other, double tol) const {
if (body_P_sensor) { if (body_P_sensor) {
if (!other.body_P_sensor if (!other.body_P_sensor
|| !assert_equal(*body_P_sensor, *other.body_P_sensor, tol)) || !assert_equal(*body_P_sensor, *other.body_P_sensor, tol))

6
gtsam/navigation/PreintegrationParams.cpp
View File

@ -27,7 +27,7 @@ namespace gtsam {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegrationParams::print(const string& s) const { void PreintegrationParams::print(const string& s) const {
PreintegratedRotation::Params::print(s); PreintegratedRotationParams::print(s);
cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]" cout << "accelerometerCovariance:\n[\n" << accelerometerCovariance << "\n]"
<< endl; << endl;
cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]" cout << "integrationCovariance:\n[\n" << integrationCovariance << "\n]"
@ -39,10 +39,10 @@ void PreintegrationParams::print(const string& s) const {
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
bool PreintegrationParams::equals(const PreintegratedRotation::Params& other, bool PreintegrationParams::equals(const PreintegratedRotationParams& other,
double tol) const { double tol) const {
auto e = dynamic_cast<const PreintegrationParams*>(&other); auto e = dynamic_cast<const PreintegrationParams*>(&other);
return e != nullptr && PreintegratedRotation::Params::equals(other, tol) && return e != nullptr && PreintegratedRotationParams::equals(other, tol) &&
use2ndOrderCoriolis == e->use2ndOrderCoriolis && use2ndOrderCoriolis == e->use2ndOrderCoriolis &&
equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance, equal_with_abs_tol(accelerometerCovariance, e->accelerometerCovariance,
tol) && tol) &&

13
gtsam/navigation/PreintegrationParams.h
View File

@ -31,7 +31,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
/// Default constructor for serialization only /// Default constructor for serialization only
PreintegrationParams() PreintegrationParams()
: accelerometerCovariance(I_3x3), : PreintegratedRotationParams(),
accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3), integrationCovariance(I_3x3),
use2ndOrderCoriolis(false), use2ndOrderCoriolis(false),
n_gravity(0, 0, -1) {} n_gravity(0, 0, -1) {}
@ -39,7 +40,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
/// The Params constructor insists on getting the navigation frame gravity vector /// The Params constructor insists on getting the navigation frame gravity vector
/// For convenience, two commonly used conventions are provided by named constructors below /// For convenience, two commonly used conventions are provided by named constructors below
PreintegrationParams(const Vector3& n_gravity) PreintegrationParams(const Vector3& n_gravity)
: accelerometerCovariance(I_3x3), : PreintegratedRotationParams(),
accelerometerCovariance(I_3x3),
integrationCovariance(I_3x3), integrationCovariance(I_3x3),
use2ndOrderCoriolis(false), use2ndOrderCoriolis(false),
n_gravity(n_gravity) {} n_gravity(n_gravity) {}
@ -54,8 +56,8 @@ struct GTSAM_EXPORT PreintegrationParams: PreintegratedRotationParams {
return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g))); return boost::shared_ptr<PreintegrationParams>(new PreintegrationParams(Vector3(0, 0, -g)));
} }
void print(const std::string& s) const; void print(const std::string& s="") const;
bool equals(const PreintegratedRotation::Params& other, double tol) const; bool equals(const PreintegratedRotationParams& other, double tol) const;
void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; } void setAccelerometerCovariance(const Matrix3& cov) { accelerometerCovariance = cov; }
void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; } void setIntegrationCovariance(const Matrix3& cov) { integrationCovariance = cov; }
@ -73,8 +75,7 @@ protected:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) { void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
namespace bs = ::boost::serialization; namespace bs = ::boost::serialization;
ar & boost::serialization::make_nvp("PreintegratedRotation_Params", ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(PreintegratedRotationParams);
boost::serialization::base_object<PreintegratedRotationParams>(*this));
ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size())); ar & bs::make_nvp("accelerometerCovariance", bs::make_array(accelerometerCovariance.data(), accelerometerCovariance.size()));
ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size())); ar & bs::make_nvp("integrationCovariance", bs::make_array(integrationCovariance.data(), integrationCovariance.size()));
ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis); ar & BOOST_SERIALIZATION_NVP(use2ndOrderCoriolis);