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

fix check.sam

release/4.3a0
Frank Dellaert 2022-02-16 08:54:36 -05:00
parent 3a49c79ee8
commit 10bf3a0199
5 changed files with 179 additions and 132 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

39
gtsam/nonlinear/factorTesting.h
View File

@ -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

32
gtsam/sam/tests/testBearingFactor.cpp
View File

@ -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);
}
/* ************************************************************************* */

32
gtsam/sam/tests/testBearingRangeFactor.cpp
View File

@ -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);
}
/* ************************************************************************* */

68
gtsam/sam/tests/testRangeFactor.cpp
View File

@ -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

140
gtsam/sam/tests/testSerializationSam.cpp Normal file
View File

@ -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);
}
/* ************************************************************************* */