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

add prior factor tests and remove TODO

release/4.3a0
Varun Agrawal 2022-05-05 11:24:26 -04:00
parent 4d275a45e6
commit 51d1c27f2d
3 changed files with 37 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/nonlinear/PriorFactor.h
View File

@ -94,7 +94,6 @@ namespace gtsam {
Vector evaluateError(const T& x, boost::optional<Matrix&> H = boost::none) const override {
if (H) (*H) = Matrix::Identity(traits<T>::GetDimension(x),traits<T>::GetDimension(x));
// manifold equivalent of z-x -> Local(x,z)
// TODO(ASL) Add Jacobians.
return -traits<T>::Local(x, prior_);
}

38
gtsam/slam/tests/testPriorFactor.cpp
View File

@ -5,12 +5,16 @@
* @date Nov 4, 2014
*/
#include <gtsam/base/Vector.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Vector.h>
#include <gtsam/navigation/ImuBias.h>
#include <gtsam/nonlinear/PriorFactor.h>
#include <gtsam/nonlinear/factorTesting.h>
using namespace std;
using namespace std::placeholders;
using namespace gtsam;
using namespace imuBias;
/* ************************************************************************* */
@ -28,11 +32,39 @@ TEST(PriorFactor, ConstructorVector3) {
// Constructor dynamic sized vector
TEST(PriorFactor, ConstructorDynamicSizeVector) {
Vector v(5); v << 1, 2, 3, 4, 5;
Vector v(5);
v << 1, 2, 3, 4, 5;
SharedNoiseModel model = noiseModel::Isotropic::Sigma(5, 1.0);
PriorFactor<Vector> factor(1, v, model);
}
Vector callEvaluateError(const PriorFactor<ConstantBias>& factor,
const ConstantBias& bias) {
return factor.evaluateError(bias);
}
// Test for imuBias::ConstantBias
TEST(PriorFactor, ConstantBias) {
Vector3 biasAcc(1, 2, 3);
Vector3 biasGyro(0.1, 0.2, 0.3);
ConstantBias bias(biasAcc, biasGyro);
PriorFactor<ConstantBias> factor(1, bias,
noiseModel::Isotropic::Sigma(6, 0.1));
Values values;
values.insert(1, bias);
EXPECT_DOUBLES_EQUAL(0.0, factor.error(values), 1e-8);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
ConstantBias incorrectBias(
(Vector6() << 1.1, 2.1, 3.1, 0.2, 0.3, 0.4).finished());
values.clear();
values.insert(1, incorrectBias);
EXPECT_DOUBLES_EQUAL(3.0, factor.error(values), 1e-8);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
int main() {
TestResult tr;