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

Merge pull request #813 from borglab/feature/between-factor-tests

release/4.3a0
Varun Agrawal 2021-07-09 18:19:29 -04:00 committed by GitHub
parent 4fc68593ac 2e40169324
commit 4ac4302871
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 58 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,18 +1,19 @@
/**
* @file testBetweenFactor.cpp
* @file testBetweenFactor.cpp
* @brief
* @author Duy-Nguyen Ta
* @date Aug 2, 2013
* @author Duy-Nguyen Ta, Varun Agrawal
* @date Aug 2, 2013
*/
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/factorTesting.h>
#include <gtsam/slam/BetweenFactor.h>
#include <boost/bind/bind.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace boost::placeholders;
using namespace gtsam;
using namespace gtsam::symbol_shorthand;
@ -48,28 +49,71 @@ TEST(BetweenFactor, Rot3) {
}
/* ************************************************************************* */
/*
// Constructor scalar
TEST(BetweenFactor, ConstructorScalar) {
SharedNoiseModel model;
double measured_value = 0.0;
BetweenFactor<double> factor(1, 2, measured_value, model);
double measured = 0.0;
BetweenFactor<double> factor(1, 2, measured, model);
}
/* ************************************************************************* */
// Constructor vector3
TEST(BetweenFactor, ConstructorVector3) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
Vector3 measured_value(1, 2, 3);
BetweenFactor<Vector3> factor(1, 2, measured_value, model);
Vector3 measured(1, 2, 3);
BetweenFactor<Vector3> factor(1, 2, measured, model);
}
/* ************************************************************************* */
// Constructor dynamic sized vector
TEST(BetweenFactor, ConstructorDynamicSizeVector) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
Vector measured_value(5); measured_value << 1, 2, 3, 4, 5;
BetweenFactor<Vector> factor(1, 2, measured_value, model);
Vector measured(5); measured << 1, 2, 3, 4, 5;
BetweenFactor<Vector> factor(1, 2, measured, model);
}
/* ************************************************************************* */
TEST(BetweenFactor, Point3Jacobians) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
Point3 measured(1, 2, 3);
BetweenFactor<Point3> factor(1, 2, measured, model);
Values values;
values.insert(1, Point3(0, 0, 0));
values.insert(2, Point3(1, 2, 3));
Vector3 error = factor.evaluateError(Point3(0, 0, 0), Point3(1, 2, 3));
EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(BetweenFactor, Rot3Jacobians) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(3, 1.0);
Rot3 measured = Rot3::Ry(M_PI_2);
BetweenFactor<Rot3> factor(1, 2, measured, model);
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3::Ry(M_PI_2));
Vector3 error = factor.evaluateError(Rot3(), Rot3::Ry(M_PI_2));
EXPECT(assert_equal<Vector3>(Vector3::Zero(), error, 1e-9));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
TEST(BetweenFactor, Pose3Jacobians) {
SharedNoiseModel model = noiseModel::Isotropic::Sigma(6, 1.0);
Pose3 measured(Rot3(), Point3(1, 2, 3));
BetweenFactor<Pose3> factor(1, 2, measured, model);
Pose3 pose1, pose2(Rot3(), Point3(1, 2, 3));
Values values;
values.insert(1, pose1);
values.insert(2, pose2);
Vector6 error = factor.evaluateError(pose1, pose2);
EXPECT(assert_equal<Vector6>(Vector6::Zero(), error, 1e-9));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
*/
/* ************************************************************************* */
int main() {