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

Symbol include, no gtsam:: needed

release/4.3a0
dellaert 2015-02-17 00:30:45 +01:00
parent 60e3ff536c
commit a375d06caa
1 changed files with 43 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

90
gtsam/slam/tests/testOrientedPlane3Factor.cpp
View File

@ -19,6 +19,7 @@
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h>
@ -37,47 +38,46 @@ GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
TEST (OrientedPlane3Factor, lm_translation_error) {
// Tests one pose, two measurements of the landmark that differ in range only.
// Normal along -x, 3m away
gtsam::Symbol lm_sym('p', 0);
gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2;
gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph;
ISAM2 isam2;
Values new_values;
NonlinearFactorGraph new_graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
gtsam::Symbol init_sym('x', 0);
gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0),
gtsam::Point3(0.0, 0.0, 0.0));
gtsam::Vector sigmas(6);
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
Vector sigmas(6);
sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001;
gtsam::PriorFactor<gtsam::Pose3> pose_prior(init_sym, init_pose,
gtsam::noiseModel::Diagonal::Sigmas(sigmas));
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
noiseModel::Diagonal::Sigmas(sigmas));
new_values.insert(init_sym, init_pose);
new_graph.add(pose_prior);
// Add two landmark measurements, differing in range
new_values.insert(lm_sym, test_lm0);
gtsam::Vector sigmas3(3);
Vector sigmas3(3);
sigmas3 << 0.1, 0.1, 0.1;
gtsam::Vector test_meas0_mean(4);
Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
OrientedPlane3Factor test_meas0(test_meas0_mean,
noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas0);
gtsam::Vector test_meas1_mean(4);
Vector test_meas1_mean(4);
test_meas1_mean << -1.0, 0.0, 0.0, 1.0;
gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
gtsam::noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
OrientedPlane3Factor test_meas1(test_meas1_mean,
noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym);
new_graph.add(test_meas1);
// Optimize
gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
gtsam::OrientedPlane3>(lm_sym);
ISAM2Result result = isam2.update(new_graph, new_values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
lm_sym);
// Given two noisy measurements of equal weight, expect result between the two
gtsam::OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
@ -85,19 +85,18 @@ TEST (OrientedPlane3Factor, lm_translation_error) {
TEST (OrientedPlane3Factor, lm_rotation_error) {
// Tests one pose, two measurements of the landmark that differ in angle only.
// Normal along -x, 3m away
gtsam::Symbol lm_sym('p', 0);
gtsam::OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
Symbol lm_sym('p', 0);
OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
gtsam::ISAM2 isam2;
gtsam::Values new_values;
gtsam::NonlinearFactorGraph new_graph;
ISAM2 isam2;
Values new_values;
NonlinearFactorGraph new_graph;
// Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose
gtsam::Symbol init_sym('x', 0);
gtsam::Pose3 init_pose(gtsam::Rot3::ypr(0.0, 0.0, 0.0),
gtsam::Point3(0.0, 0.0, 0.0));
gtsam::PriorFactor<gtsam::Pose3> pose_prior(init_sym, init_pose,
gtsam::noiseModel::Diagonal::Sigmas(
Symbol init_sym('x', 0);
Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0));
PriorFactor<Pose3> pose_prior(init_sym, init_pose,
noiseModel::Diagonal::Sigmas(
(Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished()));
new_values.insert(init_sym, init_pose);
new_graph.add(pose_prior);
@ -106,26 +105,24 @@ TEST (OrientedPlane3Factor, lm_rotation_error) {
new_values.insert(lm_sym, test_lm0);
Vector test_meas0_mean(4);
test_meas0_mean << -1.0, 0.0, 0.0, 3.0;
gtsam::OrientedPlane3Factor test_meas0(test_meas0_mean,
gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym,
lm_sym);
OrientedPlane3Factor test_meas0(test_meas0_mean,
noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym);
new_graph.add(test_meas0);
Vector test_meas1_mean(4);
test_meas1_mean << 0.0, -1.0, 0.0, 3.0;
gtsam::OrientedPlane3Factor test_meas1(test_meas1_mean,
gtsam::noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym,
lm_sym);
OrientedPlane3Factor test_meas1(test_meas1_mean,
noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym);
new_graph.add(test_meas1);
// Optimize
gtsam::ISAM2Result result = isam2.update(new_graph, new_values);
gtsam::Values result_values = isam2.calculateEstimate();
gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<
gtsam::OrientedPlane3>(lm_sym);
ISAM2Result result = isam2.update(new_graph, new_values);
Values result_values = isam2.calculateEstimate();
OrientedPlane3 optimized_plane_landmark = result_values.at<OrientedPlane3>(
lm_sym);
// Given two noisy measurements of equal weight, expect result between the two
gtsam::OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0,
-sqrt(2.0) / 2.0, 0.0, 3.0);
OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0,
0.0, 3.0);
EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
}
@ -140,8 +137,7 @@ TEST( OrientedPlane3DirectionPrior, Constructor ) {
// Factor
Key key(1);
SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas(
Vector3(0.1, 0.1, 10.0));
SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0));
OrientedPlane3DirectionPrior factor(key, planeOrientation, model);
// Create a linearization point at the zero-error point