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

3D version with Unit3 measurement now works

release/4.3a0
Frank Dellaert 2015-07-11 18:32:43 -07:00
parent 6b037ea492
commit e52dca63f6
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -20,6 +20,7 @@
#include <gtsam/geometry/Pose2.h>
#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>
@ -37,7 +38,7 @@ static SharedNoiseModel model2D(noiseModel::Isotropic::Sigma(1, 0.5));
BearingFactor2D factor2D(poseKey, pointKey, measurement2D, model2D);
typedef BearingFactor<Pose3, Point3, Unit3> BearingFactor3D;
Unit3 measurement3D(1,2,3);
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);
@ -45,9 +46,6 @@ 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);
//BOOST_CLASS_EXPORT(BearingFactor2D);
//BOOST_CLASS_EXPORT(ExpressionFactor<Rot2>);
//BOOST_CLASS_EXPORT_GUID(gtsam::ExpressionFactor<Rot2>, "ExpressionFactorRot2")
/* ************************************************************************* */
TEST(BearingFactor, Serialization2D) {
@ -70,7 +68,8 @@ TEST(BearingFactor, 2D) {
values.insert(poseKey, Pose2(1.0, 2.0, 0.57));
values.insert(pointKey, Point2(-4.0, 11.0));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */
@ -91,10 +90,11 @@ TEST(BearingFactor, 3D) {
// Set the linearization point
Values values;
values.insert(poseKey, Pose3(Rot3::RzRyRx(0.2, -0.3, 1.75), Point3(1.0, 2.0,-3.0)));
values.insert(pointKey, Point3(-2.0, 11.0, 1.0));
values.insert(poseKey, Pose3());
values.insert(pointKey, Point3(1,0,0));
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(factor.expression(), values, 1e-7,1e-5);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-7, 1e-5);
}
/* ************************************************************************* */