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

small tidy and fix unit tests

release/4.3a0
David Wisth 2021-02-16 18:21:53 +00:00
parent 960a3e1d8e
commit 5b0bd08e7b
1 changed files with 16 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam_unstable/slam/tests/testLocalOrientedPlane3Factor.cpp
View File

@ -10,28 +10,22 @@
* -------------------------------------------------------------------------- */ * -------------------------------------------------------------------------- */
/* /*
* @file testOrientedPlane3Factor.cpp * @file testLocalOrientedPlane3Factor.cpp
* @date Dec 19, 2012 * @date Feb 12, 2021
* @author Alex Trevor * @author David Wisth
* @brief Tests the OrientedPlane3Factor class * @brief Tests the LocalOrientedPlane3Factor class
*/ */
#include <gtsam_unstable/slam/LocalOrientedPlane3Factor.h> #include <gtsam_unstable/slam/LocalOrientedPlane3Factor.h>
#include <gtsam/slam/OrientedPlane3Factor.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/ISAM2.h> #include <gtsam/nonlinear/ISAM2.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std.hpp>
#include <boost/bind.hpp> #include <boost/bind.hpp>
using namespace boost::assign;
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;
@ -52,21 +46,24 @@ TEST(LocalOrientedPlane3Factor, lm_translation_error) {
// Init pose and prior. Pose Prior is needed since a single plane measurement // Init pose and prior. Pose Prior is needed since a single plane measurement
// does not fully constrain the pose // does not fully constrain the pose
Pose3 init_pose = Pose3::identity(); Pose3 init_pose = Pose3::identity();
Pose3 anchor_pose = Pose3::identity();
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
graph.addPrior(X(1), anchor_pose, noiseModel::Isotropic::Sigma(6, 0.001));
// Add two landmark measurements, differing in range // Add two landmark measurements, differing in range
Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; Vector4 measurement0(-1.0, 0.0, 0.0, 3.0);
Vector4 measurement1 {-1.0, 0.0, 0.0, 1.0}; Vector4 measurement1(-1.0, 0.0, 0.0, 1.0);
LocalOrientedPlane3Factor factor0( LocalOrientedPlane3Factor factor0(
measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); measurement0, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0));
LocalOrientedPlane3Factor factor1( LocalOrientedPlane3Factor factor1(
measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); measurement1, noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(1), P(0));
graph.add(factor0); graph.add(factor0);
graph.add(factor1); graph.add(factor1);
// Initial Estimate // Initial Estimate
Values values; Values values;
values.insert(X(0), init_pose); values.insert(X(0), init_pose);
values.insert(X(1), anchor_pose);
values.insert(P(0), test_lm0); values.insert(P(0), test_lm0);
// Optimize // Optimize
@ -94,8 +91,8 @@ TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001)); graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
// Add two landmark measurements, differing in angle // Add two landmark measurements, differing in angle
Vector4 measurement0 {-1.0, 0.0, 0.0, 3.0}; Vector4 measurement0(-1.0, 0.0, 0.0, 3.0);
Vector4 measurement1 {0.0, -1.0, 0.0, 3.0}; Vector4 measurement1(0.0, -1.0, 0.0, 3.0);
LocalOrientedPlane3Factor factor0(measurement0, LocalOrientedPlane3Factor factor0(measurement0,
noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0)); noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0));
LocalOrientedPlane3Factor factor1(measurement1, LocalOrientedPlane3Factor factor1(measurement1,
@ -220,8 +217,9 @@ TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
// Optimize // Optimize
try { try {
GaussNewtonOptimizer optimizer(graph, initialEstimate); ISAM2 isam2;
Values result = optimizer.optimize(); isam2.update(graph, initialEstimate);
Values result = isam2.calculateEstimate();
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
EXPECT(x0.equals(result.at<Pose3>(X(0)))); EXPECT(x0.equals(result.at<Pose3>(X(0))));
EXPECT(p1_in_x1.equals(result.at<Plane>(P(1)))); EXPECT(p1_in_x1.equals(result.at<Plane>(P(1))));