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

fixed OrientedPlane3Factor.

release/4.3a0
nsrinivasan7 2015-02-12 07:48:20 -05:00
parent 493a4f7f86
commit e4a6a2a951
2 changed files with 13 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/OrientedPlane3Factor.h
View File

@ -55,7 +55,7 @@ namespace gtsam {
boost::optional<Matrix&> H2 = boost::none) const boost::optional<Matrix&> H2 = boost::none) const
{ {
OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2); OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2);
Vector err; Vector err(3);
err << predicted_plane.error (measured_p_); err << predicted_plane.error (measured_p_);
return (err); return (err);
}; };

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

@ -78,13 +78,13 @@ TEST (OrientedPlane3, lm_translation_error)
new_graph.add (test_meas1); new_graph.add (test_meas1);
// Optimize // Optimize
// gtsam::ISAM2Result result = isam2.update (new_graph, new_values); gtsam::ISAM2Result result = isam2.update (new_graph, new_values);
// gtsam::Values result_values = isam2.calculateEstimate (); gtsam::Values result_values = isam2.calculateEstimate ();
// gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym); gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym);
// // Given two noisy measurements of equal weight, expect result between the two // 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); gtsam::OrientedPlane3 expected_plane_landmark (-1.0, 0.0, 0.0, 2.0);
// EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark));
} }
TEST (OrientedPlane3, lm_rotation_error) TEST (OrientedPlane3, lm_rotation_error)
@ -113,18 +113,18 @@ TEST (OrientedPlane3, lm_rotation_error)
gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym);
new_graph.add (test_meas0); new_graph.add (test_meas0);
Vector test_meas1_mean(4); Vector test_meas1_mean(4);
test_meas1_mean << -1.0, 0.0, 0.0, 3.0; 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); gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym);
new_graph.add (test_meas1); new_graph.add (test_meas1);
// Optimize // Optimize
// gtsam::ISAM2Result result = isam2.update (new_graph, new_values); gtsam::ISAM2Result result = isam2.update (new_graph, new_values);
// gtsam::Values result_values = isam2.calculateEstimate (); gtsam::Values result_values = isam2.calculateEstimate ();
// gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym); gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at<gtsam::OrientedPlane3>(lm_sym);
// Given two noisy measurements of equal weight, expect result between the two // 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); gtsam::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)); EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark));
} }
// ************************************************************************* // *************************************************************************