fix check.sam
Frank Dellaert
parent
3a49c79ee8
commit
10bf3a0199
|
|
@ -21,6 +21,8 @@
|
|||
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -34,13 +36,14 @@ namespace gtsam {
|
|||
* This is fixable but expensive, and does not matter in practice as most factors will sit near
|
||||
* zero errors anyway. However, it means that below will only be exact for the correct measurement.
|
||||
*/
|
||||
JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
|
||||
const Values& values, double delta = 1e-5) {
|
||||
inline JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
|
||||
const Values& values,
|
||||
double delta = 1e-5) {
|
||||
// We will fill a vector of key/Jacobians pairs (a map would sort)
|
||||
std::vector<std::pair<Key, Matrix> > jacobians;
|
||||
|
||||
// Get size
|
||||
const Eigen::VectorXd e = factor.whitenedError(values);
|
||||
const Vector e = factor.whitenedError(values);
|
||||
const size_t rows = e.size();
|
||||
|
||||
// Loop over all variables
|
||||
|
|
@ -51,18 +54,18 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
|
|||
const size_t cols = dX.dim(key);
|
||||
Matrix J = Matrix::Zero(rows, cols);
|
||||
for (size_t col = 0; col < cols; ++col) {
|
||||
Eigen::VectorXd dx = Eigen::VectorXd::Zero(cols);
|
||||
Vector dx = Vector::Zero(cols);
|
||||
dx(col) = delta;
|
||||
dX[key] = dx;
|
||||
Values eval_values = values.retract(dX);
|
||||
const Eigen::VectorXd left = factor.whitenedError(eval_values);
|
||||
const Vector left = factor.whitenedError(eval_values);
|
||||
dx(col) = -delta;
|
||||
dX[key] = dx;
|
||||
eval_values = values.retract(dX);
|
||||
const Eigen::VectorXd right = factor.whitenedError(eval_values);
|
||||
const Vector right = factor.whitenedError(eval_values);
|
||||
J.col(col) = (left - right) * one_over_2delta;
|
||||
}
|
||||
jacobians.push_back(std::make_pair(key, J));
|
||||
jacobians.emplace_back(key, J);
|
||||
}
|
||||
|
||||
// Next step...return JacobianFactor
|
||||
|
|
@ -71,15 +74,15 @@ JacobianFactor linearizeNumerically(const NoiseModelFactor& factor,
|
|||
|
||||
namespace internal {
|
||||
// CPPUnitLite-style test for linearization of a factor
|
||||
bool testFactorJacobians(const std::string& name_,
|
||||
const NoiseModelFactor& factor, const gtsam::Values& values, double delta,
|
||||
double tolerance) {
|
||||
|
||||
inline bool testFactorJacobians(const std::string& name_,
|
||||
const NoiseModelFactor& factor,
|
||||
const gtsam::Values& values, double delta,
|
||||
double tolerance) {
|
||||
// Create expected value by numerical differentiation
|
||||
JacobianFactor expected = linearizeNumerically(factor, values, delta);
|
||||
|
||||
// Create actual value by linearize
|
||||
boost::shared_ptr<JacobianFactor> actual = //
|
||||
auto actual =
|
||||
boost::dynamic_pointer_cast<JacobianFactor>(factor.linearize(values));
|
||||
if (!actual) return false;
|
||||
|
||||
|
|
@ -89,17 +92,19 @@ bool testFactorJacobians(const std::string& name_,
|
|||
// if not equal, test individual jacobians:
|
||||
if (!equal) {
|
||||
for (size_t i = 0; i < actual->size(); i++) {
|
||||
bool i_good = assert_equal((Matrix) (expected.getA(expected.begin() + i)),
|
||||
(Matrix) (actual->getA(actual->begin() + i)), tolerance);
|
||||
bool i_good =
|
||||
assert_equal((Matrix)(expected.getA(expected.begin() + i)),
|
||||
(Matrix)(actual->getA(actual->begin() + i)), tolerance);
|
||||
if (!i_good) {
|
||||
std::cout << "Mismatch in Jacobian " << i+1 << " (base 1), as shown above" << std::endl;
|
||||
std::cout << "Mismatch in Jacobian " << i + 1
|
||||
<< " (base 1), as shown above" << std::endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return equal;
|
||||
}
|
||||
}
|
||||
} // namespace internal
|
||||
|
||||
/// \brief Check the Jacobians produced by a factor against finite differences.
|
||||
/// \param factor The factor to test.
|
||||
|
|
@ -109,4 +114,4 @@ bool testFactorJacobians(const std::string& name_,
|
|||
#define EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, numerical_derivative_step, tolerance) \
|
||||
{ EXPECT(gtsam::internal::testFactorJacobians(name_, factor, values, numerical_derivative_step, tolerance)); }
|
||||
|
||||
} // namespace gtsam
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
|
|
@ -21,14 +21,13 @@
|
|||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/nonlinear/expressionTesting.h>
|
||||
#include <gtsam/base/serialization.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace {
|
||||
Key poseKey(1);
|
||||
Key pointKey(2);
|
||||
|
||||
|
|
@ -41,43 +40,18 @@ typedef BearingFactor<Pose3, Point3> BearingFactor3D;
|
|||
Unit3 measurement3D = Pose3().bearing(Point3(1, 0, 0)); // has to match values!
|
||||
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
|
||||
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Export Noisemodels
|
||||
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
|
||||
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingFactor, Serialization2D) {
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingFactor, 2D) {
|
||||
// Serialize the factor
|
||||
std::string serialized = serializeXML(factor2D);
|
||||
|
||||
// And de-serialize it
|
||||
BearingFactor2D factor;
|
||||
deserializeXML(serialized, factor);
|
||||
|
||||
// Set the linearization point
|
||||
Values values;
|
||||
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
|
||||
values.insert(pointKey, Point2(-4.0, 11.0));
|
||||
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
|
||||
values, 1e-7, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingFactor, Serialization3D) {
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -21,14 +21,13 @@
|
|||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/nonlinear/expressionTesting.h>
|
||||
#include <gtsam/base/serialization.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace {
|
||||
Key poseKey(1);
|
||||
Key pointKey(2);
|
||||
|
||||
|
|
@ -40,43 +39,18 @@ typedef BearingRangeFactor<Pose3, Point3> BearingRangeFactor3D;
|
|||
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
|
||||
BearingRangeFactor3D factor3D(poseKey, pointKey,
|
||||
Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Export Noisemodels
|
||||
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
|
||||
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingRangeFactor, Serialization2D) {
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingRangeFactor, 2D) {
|
||||
// Serialize the factor
|
||||
std::string serialized = serializeXML(factor2D);
|
||||
|
||||
// And de-serialize it
|
||||
BearingRangeFactor2D factor;
|
||||
deserializeXML(serialized, factor);
|
||||
|
||||
// Set the linearization point
|
||||
Values values;
|
||||
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
|
||||
values.insert(pointKey, Point2(-4.0, 11.0));
|
||||
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression({poseKey, pointKey}),
|
||||
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor2D.expression({poseKey, pointKey}),
|
||||
values, 1e-7, 1e-5);
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingRangeFactor, Serialization3D) {
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
|
||||
EXPECT_CORRECT_FACTOR_JACOBIANS(factor2D, values, 1e-7, 1e-5);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -22,7 +22,6 @@
|
|||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -32,42 +31,40 @@ using namespace std::placeholders;
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// Create a noise model for the pixel error
|
||||
static SharedNoiseModel model(noiseModel::Unit::Create(1));
|
||||
|
||||
typedef RangeFactor<Pose2, Point2> RangeFactor2D;
|
||||
typedef RangeFactor<Pose3, Point3> RangeFactor3D;
|
||||
typedef RangeFactorWithTransform<Pose2, Point2> RangeFactorWithTransform2D;
|
||||
typedef RangeFactorWithTransform<Pose3, Point3> RangeFactorWithTransform3D;
|
||||
|
||||
// Keys are deliberately *not* in sorted order to test that case.
|
||||
namespace {
|
||||
// Create a noise model for the pixel error
|
||||
static SharedNoiseModel model(noiseModel::Unit::Create(1));
|
||||
|
||||
constexpr Key poseKey(2);
|
||||
constexpr Key pointKey(1);
|
||||
constexpr double measurement(10.0);
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector factorError2D(const Pose2& pose, const Point2& point,
|
||||
const RangeFactor2D& factor) {
|
||||
const RangeFactor2D& factor) {
|
||||
return factor.evaluateError(pose, point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector factorError3D(const Pose3& pose, const Point3& point,
|
||||
const RangeFactor3D& factor) {
|
||||
const RangeFactor3D& factor) {
|
||||
return factor.evaluateError(pose, point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector factorErrorWithTransform2D(const Pose2& pose, const Point2& point,
|
||||
const RangeFactorWithTransform2D& factor) {
|
||||
const RangeFactorWithTransform2D& factor) {
|
||||
return factor.evaluateError(pose, point);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector factorErrorWithTransform3D(const Pose3& pose, const Point3& point,
|
||||
const RangeFactorWithTransform3D& factor) {
|
||||
const RangeFactorWithTransform3D& factor) {
|
||||
return factor.evaluateError(pose, point);
|
||||
}
|
||||
} // namespace
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, Constructor) {
|
||||
|
|
@ -75,27 +72,6 @@ TEST( RangeFactor, Constructor) {
|
|||
RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Export Noisemodels
|
||||
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
|
||||
BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(RangeFactor, Serialization2D) {
|
||||
RangeFactor2D factor2D(poseKey, pointKey, measurement, model);
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(RangeFactor, Serialization3D) {
|
||||
RangeFactor3D factor3D(poseKey, pointKey, measurement, model);
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, ConstructorWithTransform) {
|
||||
Pose2 body_P_sensor_2D(0.25, -0.10, -M_PI_2);
|
||||
|
|
@ -142,28 +118,6 @@ TEST( RangeFactor, EqualsWithTransform ) {
|
|||
body_P_sensor_3D);
|
||||
CHECK(assert_equal(factor3D_1, factor3D_2));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, EqualsAfterDeserializing) {
|
||||
// Check that the same results are obtained after deserializing:
|
||||
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
|
||||
Point3(0.25, -0.10, 1.0));
|
||||
|
||||
RangeFactorWithTransform3D factor3D_1(poseKey, pointKey, measurement, model,
|
||||
body_P_sensor_3D), factor3D_2;
|
||||
|
||||
// check with Equal() trait:
|
||||
gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
|
||||
CHECK(assert_equal(factor3D_1, factor3D_2));
|
||||
|
||||
const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
|
||||
const Point3 point(-2.0, 11.0, 1.0);
|
||||
const Values values = {{poseKey, genericValue(pose)}, {pointKey, genericValue(point)}};
|
||||
|
||||
const Vector error_1 = factor3D_1.unwhitenedError(values);
|
||||
const Vector error_2 = factor3D_2.unwhitenedError(values);
|
||||
CHECK(assert_equal(error_1, error_2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( RangeFactor, Error2D ) {
|
||||
// Create a factor
|
||||
|
|
@ -411,7 +365,7 @@ TEST( RangeFactor, Camera) {
|
|||
/* ************************************************************************* */
|
||||
// Do a test with non GTSAM types
|
||||
|
||||
namespace gtsam{
|
||||
namespace gtsam {
|
||||
template <>
|
||||
struct Range<Vector4, Vector4> {
|
||||
typedef double result_type;
|
||||
|
|
@ -421,7 +375,7 @@ struct Range<Vector4, Vector4> {
|
|||
// derivatives not implemented
|
||||
}
|
||||
};
|
||||
}
|
||||
} // namespace gtsam
|
||||
|
||||
TEST(RangeFactor, NonGTSAM) {
|
||||
// Create a factor
|
||||
|
|
|
|||
|
|
@ -0,0 +1,140 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testSerializationSam.cpp
|
||||
* @brief All serialization test in this directory
|
||||
* @author Frank Dellaert
|
||||
* @date February 2022
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam/base/serialization.h>
|
||||
#include <gtsam/base/serializationTestHelpers.h>
|
||||
#include <gtsam/geometry/Pose2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/nonlinear/expressionTesting.h>
|
||||
#include <gtsam/nonlinear/factorTesting.h>
|
||||
#include <gtsam/sam/BearingFactor.h>
|
||||
#include <gtsam/sam/BearingRangeFactor.h>
|
||||
#include <gtsam/sam/RangeFactor.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
namespace {
|
||||
Key poseKey(1);
|
||||
Key pointKey(2);
|
||||
constexpr double rangeMmeasurement(10.0);
|
||||
} // namespace
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Export Noisemodels
|
||||
// See http://www.boost.org/doc/libs/1_32_0/libs/serialization/doc/special.html
|
||||
BOOST_CLASS_EXPORT(gtsam::noiseModel::Isotropic);
|
||||
BOOST_CLASS_EXPORT(gtsam::noiseModel::Unit);
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SerializationSam, BearingFactor2D) {
|
||||
using BearingFactor2D = BearingFactor<Pose2, Point2>;
|
||||
double measurement2D(10.0);
|
||||
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
|
||||
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SerializationSam, BearingFactor3D) {
|
||||
using BearingFactor3D = BearingFactor<Pose3, Point3>;
|
||||
Unit3 measurement3D =
|
||||
Pose3().bearing(Point3(1, 0, 0)); // has to match values!
|
||||
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(2, 0.5));
|
||||
BearingFactor3D factor3D(poseKey, pointKey, measurement3D, model3D);
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
namespace {
|
||||
static SharedNoiseModel rangeNoiseModel(noiseModel::Unit::Create(1));
|
||||
}
|
||||
|
||||
TEST(SerializationSam, RangeFactor2D) {
|
||||
using RangeFactor2D = RangeFactor<Pose2, Point2>;
|
||||
RangeFactor2D factor2D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(SerializationSam, RangeFactor3D) {
|
||||
using RangeFactor3D = RangeFactor<Pose3, Point3>;
|
||||
RangeFactor3D factor3D(poseKey, pointKey, rangeMmeasurement, rangeNoiseModel);
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(RangeFactor, EqualsAfterDeserializing) {
|
||||
// Check that the same results are obtained after deserializing:
|
||||
Pose3 body_P_sensor_3D(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2),
|
||||
Point3(0.25, -0.10, 1.0));
|
||||
RangeFactorWithTransform<Pose3, Point3> factor3D_1(
|
||||
poseKey, pointKey, rangeMmeasurement, rangeNoiseModel, body_P_sensor_3D),
|
||||
factor3D_2;
|
||||
|
||||
// check with Equal() trait:
|
||||
gtsam::serializationTestHelpers::roundtripXML(factor3D_1, factor3D_2);
|
||||
CHECK(assert_equal(factor3D_1, factor3D_2));
|
||||
|
||||
const Pose3 pose(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0, -3.0));
|
||||
const Point3 point(-2.0, 11.0, 1.0);
|
||||
const Values values = {{poseKey, genericValue(pose)},
|
||||
{pointKey, genericValue(point)}};
|
||||
|
||||
const Vector error_1 = factor3D_1.unwhitenedError(values);
|
||||
const Vector error_2 = factor3D_2.unwhitenedError(values);
|
||||
CHECK(assert_equal(error_1, error_2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingRangeFactor, Serialization2D) {
|
||||
using BearingRangeFactor2D = BearingRangeFactor<Pose2, Point2>;
|
||||
static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(2, 0.5));
|
||||
BearingRangeFactor2D factor2D(poseKey, pointKey, 1, 2, model2D);
|
||||
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor2D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor2D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(BearingRangeFactor, Serialization3D) {
|
||||
using BearingRangeFactor3D = BearingRangeFactor<Pose3, Point3>;
|
||||
static SharedNoiseModel model3D(noiseModel::Isotropic::Sigma(3, 0.5));
|
||||
BearingRangeFactor3D factor3D(poseKey, pointKey,
|
||||
Pose3().bearing(Point3(1, 0, 0)), 1, model3D);
|
||||
EXPECT(serializationTestHelpers::equalsObj(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsXML(factor3D));
|
||||
EXPECT(serializationTestHelpers::equalsBinary(factor3D));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Loading…
Reference in New Issue