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

fixed check.navigation

release/4.3a0
Frank Dellaert 2022-02-16 09:39:22 -05:00
parent 8e33e96efa
commit f71f12498d
4 changed files with 43 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -19,8 +19,6 @@
#include <gtsam/navigation/AttitudeFactor.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/serialization.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
@ -63,22 +61,6 @@ TEST( Rot3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
// Export Noisemodels
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic)
/* ************************************************************************* */
TEST(Rot3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Rot3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
TEST(Rot3AttitudeFactor, CopyAndMove) {
Unit3 nDown(0, 0, -1);
@ -129,17 +111,6 @@ TEST( Pose3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */
TEST(Pose3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Pose3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
TEST(Pose3AttitudeFactor, CopyAndMove) {
Unit3 nDown(0, 0, -1);

5
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -31,7 +31,7 @@ using namespace std;
using namespace gtsam;
using namespace GeographicLib;
// *************************************************************************
namespace {
// Convert from Mag to ENU
// ENU Origin is where the plane was in hold next to runway
// const double lat0 = 33.86998, lon0 = -84.30626, h0 = 274;
@ -51,10 +51,11 @@ Point3 bias(10, -10, 50);
Point3 scaled = scale * nM;
Point3 measured = nRb.inverse() * (scale * nM) + bias;
double s(scale * nM.norm());
double s(scale* nM.norm());
Unit3 dir(nM);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 0.25);
} // namespace
using boost::none;

7
gtsam/navigation/tests/testMagPoseFactor.cpp
View File

@ -20,7 +20,7 @@
using namespace std::placeholders;
using namespace gtsam;
// *****************************************************************************
namespace {
// Magnetic field in the nav frame (NED), with units of nT.
Point3 nM(22653.29982, -1956.83010, 44202.47862);
@ -51,8 +51,9 @@ SharedNoiseModel model3 = noiseModel::Isotropic::Sigma(3, 0.25);
// Make up a rotation and offset of the sensor in the body frame.
Pose2 body_P2_sensor(Rot2(-0.30), Point2(1.0, -2.0));
Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)), Point3(-0.1, 0.2, 0.3));
// *****************************************************************************
Pose3 body_P3_sensor(Rot3::RzRyRx(Vector3(1.5, 0.9, -1.15)),
Point3(-0.1, 0.2, 0.3));
} // namespace
// *****************************************************************************
TEST(MagPoseFactor, Constructors) {

49
gtsam/navigation/tests/testImuFactorSerialization.cpp → gtsam/navigation/tests/testSerializationNavigation.cpp
View File

@ -10,17 +10,19 @@
* -------------------------------------------------------------------------- */
/**
* @file testImuFactor.cpp
* @brief Unit test for ImuFactor
* @file testSerializationNavigation.cpp
* @brief serialization tests for navigation
* @author Luca Carlone
* @author Frank Dellaert
* @author Richard Roberts
* @author Stephen Williams
* @author Varun Agrawal
* @date February 2022
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <gtsam/navigation/AttitudeFactor.h>
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuFactor.h>
@ -30,17 +32,13 @@ using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
"gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
"gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
"gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
"gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal")
BOOST_CLASS_EXPORT_GUID(noiseModel::Constrained, "gtsam_noiseModel_Constrained")
BOOST_CLASS_EXPORT_GUID(noiseModel::Diagonal, "gtsam_noiseModel_Diagonal")
BOOST_CLASS_EXPORT_GUID(noiseModel::Gaussian, "gtsam_noiseModel_Gaussian")
BOOST_CLASS_EXPORT_GUID(noiseModel::Unit, "gtsam_noiseModel_Unit")
BOOST_CLASS_EXPORT_GUID(noiseModel::Isotropic, "gtsam_noiseModel_Isotropic")
BOOST_CLASS_EXPORT_GUID(SharedNoiseModel, "gtsam_SharedNoiseModel")
BOOST_CLASS_EXPORT_GUID(SharedDiagonal, "gtsam_SharedDiagonal")
template <typename P>
P getPreintegratedMeasurements() {
@ -69,6 +67,7 @@ P getPreintegratedMeasurements() {
return pim;
}
/* ************************************************************************* */
TEST(ImuFactor, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
@ -83,6 +82,7 @@ TEST(ImuFactor, serialization) {
EXPECT(equalsBinary<ImuFactor>(factor));
}
/* ************************************************************************* */
TEST(ImuFactor2, serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedImuMeasurements>();
@ -93,6 +93,7 @@ TEST(ImuFactor2, serialization) {
EXPECT(equalsBinary<ImuFactor2>(factor));
}
/* ************************************************************************* */
TEST(CombinedImuFactor, Serialization) {
auto pim = getPreintegratedMeasurements<PreintegratedCombinedMeasurements>();
@ -107,6 +108,28 @@ TEST(CombinedImuFactor, Serialization) {
EXPECT(equalsBinary<CombinedImuFactor>(factor));
}
/* ************************************************************************* */
TEST(Rot3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Rot3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
TEST(Pose3AttitudeFactor, Serialization) {
Unit3 nDown(0, 0, -1);
SharedNoiseModel model = noiseModel::Isotropic::Sigma(2, 0.25);
Pose3AttitudeFactor factor(0, nDown, model);
EXPECT(serializationTestHelpers::equalsObj(factor));
EXPECT(serializationTestHelpers::equalsXML(factor));
EXPECT(serializationTestHelpers::equalsBinary(factor));
}
/* ************************************************************************* */
int main() {
TestResult tr;