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

flushing out more compilation errors in tests

release/4.3a0
Mike Bosse 2014-12-20 11:54:08 +01:00
parent 99d2203617
commit 6e5dbcf2a3
15 changed files with 73 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
gtsam/base/Testable.h
View File

@ -73,6 +73,12 @@ namespace gtsam {
inline void print(const T& object, const std::string& s = "") {
object.print(s);
}
inline void print(float v, const std::string& s = "") {
printf("%s%f\n",s.c_str(),v);
}
inline void print(double v, const std::string& s = "") {
printf("%s%lf\n",s.c_str(),v);
}
/** Call equal on the object */
template<class T>

6
gtsam/base/concepts.h
View File

@ -368,9 +368,9 @@ struct traits_x<float> : public internal::ScalarTraits<float> {};
// traits for any double Eigen matrix
template<int M, int N, int Options, int MaxRows, int MaxCols>
struct traits_x< Eigen::Matrix<double, M, N, Options, MaxRows, MaxCols> > {
BOOST_STATIC_ASSERT_MSG(
M != Eigen::Dynamic && N != Eigen::Dynamic,
"These traits are only valid on fixed-size types.");
// BOOST_STATIC_ASSERT_MSG(
// M != Eigen::Dynamic && N != Eigen::Dynamic,
// "These traits are only valid on fixed-size types.");
// Typedefs required by all manifold types.
typedef vector_space_tag structure_category;

4
gtsam/base/numericalDerivative.h
View File

@ -201,8 +201,8 @@ typename internal::FixedSizeMatrix<Y,X1>::type numericalDerivative21(Y (*h)(cons
template<class Y, class X1, class X2>
typename internal::FixedSizeMatrix<Y,X2>::type numericalDerivative22(boost::function<Y(const X1&, const X2&)> h,
const X1& x1, const X2& x2, double delta = 1e-5) {
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<X1>::structure_category>::value),
"Template argument X1 must be a manifold type.");
// BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<X1>::structure_category>::value),
// "Template argument X1 must be a manifold type.");
BOOST_STATIC_ASSERT_MSG( (boost::is_base_of<gtsam::manifold_tag, typename traits_x<X2>::structure_category>::value),
"Template argument X2 must be a manifold type.");
return numericalDerivative11<Y, X2>(boost::bind(h, x1, _1), x2, delta);

5
gtsam/geometry/PinholeCamera.h
View File

@ -197,6 +197,11 @@ public:
return d;
}
/// for Canonical
static PinholeCamera identity() {
return PinholeCamera(); // assumes that the default constructor is valid
}
/// @}
/// @name Transformations and measurement functions
/// @{

10
gtsam/geometry/Rot3.h
View File

@ -276,7 +276,15 @@ namespace gtsam {
/// Returns local retract coordinates \f$ [R_x,R_y,R_z] \f$ in neighborhood around current rotation
Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
Vector3 localCoordinates(const Rot3& t2, OptionalJacobian<3,3> Horigin, OptionalJacobian<3,3> H2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const {
if (Horigin) {
CONCEPT_NOT_IMPLEMENTED;
}
if (H2) {
CONCEPT_NOT_IMPLEMENTED;
}
return localCoordinates(t2,mode);
}
/// @}
/// @name Lie Group
/// @{

2
gtsam/navigation/tests/testAHRSFactor.cpp
View File

@ -139,7 +139,7 @@ TEST(AHRSFactor, Error) {
// Expected error
Vector3 errorExpected(3);
errorExpected << 0, 0, 0;
EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
EXPECT(assert_equal(Vector(errorExpected), Vector(errorActual), 1e-6));
// Expected Jacobians
Matrix H1e = numericalDerivative11<Vector3, Rot3>(

18
gtsam/nonlinear/tests/testValues.cpp
View File

@ -55,13 +55,27 @@ int TestValueData::DestructorCount = 0;
class TestValue {
TestValueData data_;
public:
enum {dimension = 0};
void print(const std::string& str = "") const {}
bool equals(const TestValue& other, double tol = 1e-9) const { return true; }
size_t dim() const { return 0; }
TestValue retract(const Vector&) const { return TestValue(); }
Vector localCoordinates(const TestValue&) const { return Vector(); }
TestValue retract(const Vector&,
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const {
return TestValue();
}
Vector localCoordinates(const TestValue&,
OptionalJacobian<dimension,dimension> H1=boost::none,
OptionalJacobian<dimension,dimension> H2=boost::none) const {
return Vector();
}
};
namespace gtsam {
template <> struct traits_x<TestValue> : public internal::Manifold<TestValue> {};
}
/* ************************************************************************* */
TEST( Values, equals1 )
{

3
gtsam/slam/BetweenFactor.h
View File

@ -93,7 +93,8 @@ namespace gtsam {
boost::none) const {
T hx = traits_x<T>::Between(p1, p2, H1, H2); // h(x)
// manifold equivalent of h(x)-z -> log(z,h(x))
OptionalJacobian<traits_x<T>::dimension, traits_x<T>::dimension> Hlocal;
typename traits_x<T>::ChartJacobian Hlocal;
//OptionalJacobian<traits_x<T>::dimension, traits_x<T>::dimension> Hlocal;
Vector rval = traits_x<T>::Local(measured_, hx, boost::none, Hlocal);
(*H1) = ((*Hlocal) * (*H1)).eval();
(*H2) = ((*Hlocal) * (*H2)).eval();

2
gtsam/slam/SmartFactorBase.h
View File

@ -48,7 +48,7 @@ protected:
boost::optional<POSE> body_P_sensor_; ///< The pose of the sensor in the body frame (one for all cameras)
static const int ZDim = traits::dimension<Z>::value; ///< Measurement dimension
static const int ZDim = traits_x<Z>::dimension; ///< Measurement dimension
/// Definitions for blocks of F
typedef Eigen::Matrix<double, ZDim, D> Matrix2D; // F

2
gtsam/slam/SmartProjectionFactor.h
View File

@ -104,7 +104,7 @@ protected:
/// shorthand for this class
typedef SmartProjectionFactor<POSE, CALIBRATION, D> This;
static const int ZDim = traits::dimension<Point2>::value; ///< Measurement dimension
static const int ZDim = traits_x<Point2>::dimension; ///< Measurement dimension
public:

4
gtsam/slam/tests/testBetweenFactor.cpp
View File

@ -31,13 +31,13 @@ TEST(BetweenFactor, Rot3) {
Vector expected = Rot3::Logmap(measured.inverse() * R1.between(R2));
EXPECT(assert_equal(expected,actual/*, 1e-100*/)); // Uncomment to make unit test fail
Matrix numericalH1 = numericalDerivative21(
Matrix numericalH1 = numericalDerivative21<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
Matrix numericalH2 = numericalDerivative22(
Matrix numericalH2 = numericalDerivative22<Vector3,Rot3,Rot3>(
boost::function<Vector(const Rot3&, const Rot3&)>(boost::bind(
&BetweenFactor<Rot3>::evaluateError, factor, _1, _2, boost::none,
boost::none)), R1, R2, 1e-5);

4
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) {
CHECK(assert_equal(expected, actual, 1e-8));
// Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(
Matrix expectedH1 = numericalDerivative11<Vector3,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(
Matrix expectedH2 = numericalDerivative11<Vector3,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2);

19
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -96,7 +96,8 @@ TEST (EssentialMatrixFactor, factor) {
// Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected;
Hexpected = numericalDerivative11<Vector, EssentialMatrix>(
typedef Eigen::Matrix<double,1,1> Vector1;
Hexpected = numericalDerivative11<Vector1, EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE);
@ -173,8 +174,8 @@ TEST (EssentialMatrixFactor2, factor) {
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -247,8 +248,8 @@ TEST (EssentialMatrixFactor3, factor) {
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -389,8 +390,8 @@ TEST (EssentialMatrixFactor2, extraTest) {
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -458,8 +459,8 @@ TEST (EssentialMatrixFactor3, extraTest) {
boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none);
Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected1 = numericalDerivative21<Vector2, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<Vector2, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));

10
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -53,7 +53,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
Pose3RotationPrior factor(poseKey, rot3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3RotationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -67,7 +67,7 @@ TEST( testPoseRotationFactor, level3_error ) {
#else
EXPECT(assert_equal(Vector3(-0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2));
#endif
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3RotationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
// the derivative is more complex, but is close to the identity for Rot3 around the origin
// If not using true expmap will be close, but not exact around the origin
// EXPECT(assert_equal(expH1, actH1, tol));
@ -79,7 +79,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) {
Pose2RotationPrior factor(poseKey, rot2A, model1);
Matrix actH1;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector2,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -89,7 +89,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -M_PI_2).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector2,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -99,7 +99,7 @@ TEST( testPoseRotationFactor, level2_error_wrap ) {
Pose2RotationPrior factor(poseKey, rot2D, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -0.02).finished(), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector2,Pose2RotationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}

16
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -49,7 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -59,7 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -69,7 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -79,7 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -89,7 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -99,7 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1;
EXPECT(assert_equal(Vector3(-3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector3,Pose3TranslationPrior,Pose3>(evalFactorError3, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -109,7 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) {
Pose2TranslationPrior factor(poseKey, point2A, model2);
Matrix actH1;
EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector2,Pose2TranslationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
@ -119,7 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1;
EXPECT(assert_equal(Vector2(-3.0,-4.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
Matrix expH1 = numericalDerivative22<Vector2,Pose2TranslationPrior,Pose2>(evalFactorError2, factor, pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}