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

Unit tests for camera range factors

release/4.3a0
Richard Roberts 2012-03-24 20:49:35 +00:00
parent df1b9c32c1
commit aa7eb6306e
2 changed files with 54 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/slam/tests/testGeneralSFMFactor.cpp
View File

@ -21,6 +21,7 @@ using namespace boost;
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/slam/GeneralSFMFactor.h> #include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/RangeFactor.h> #include <gtsam/slam/RangeFactor.h>
#include <gtsam/slam/PriorFactor.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -359,6 +360,55 @@ TEST( GeneralSFMFactor, optimize_varK_BA ) {
EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements); EXPECT(optimizer->error() < 0.5 * reproj_error * nMeasurements);
} }
/* ************************************************************************* */
TEST(GeneralSFMFactor, GeneralCameraPoseRange) {
// Tests range factor between a GeneralCamera and a Pose3
Graph graph;
graph.addCameraConstraint(0, GeneralCamera());
graph.add(RangeFactor<GeneralCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
Values init;
init.insert(Symbol('x',0), GeneralCamera());
init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
// The optimal value between the 2m range factor and 1m prior is 1.5m
Values expected;
expected.insert(Symbol('x',0), GeneralCamera());
expected.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.5,0.0,0.0)));
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9;
params.relativeErrorTol = 1e-9;
Values actual = *LevenbergMarquardtOptimizer(graph, init, params).optimized();
EXPECT(assert_equal(expected, actual, 1e-4));
}
/* ************************************************************************* */
TEST(GeneralSFMFactor, CalibratedCameraPoseRange) {
// Tests range factor between a CalibratedCamera and a Pose3
NonlinearFactorGraph graph;
graph.add(PriorFactor<CalibratedCamera>(Symbol('x',0), CalibratedCamera(), sharedSigma(6, 1.0)));
graph.add(RangeFactor<CalibratedCamera, Pose3>(Symbol('x',0), Symbol('x',1), 2.0, sharedSigma(1, 1.0)));
graph.add(PriorFactor<Pose3>(Symbol('x',1), Pose3(Rot3(), Point3(1.0, 0.0, 0.0)), sharedSigma(6, 1.0)));
Values init;
init.insert(Symbol('x',0), CalibratedCamera());
init.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.0,1.0,1.0)));
Values expected;
expected.insert(Symbol('x',0), CalibratedCamera(Pose3(Rot3(), Point3(-0.333333333333, 0, 0))));
expected.insert(Symbol('x',1), Pose3(Rot3(), Point3(1.333333333333, 0, 0)));
LevenbergMarquardtParams params;
params.absoluteErrorTol = 1e-9;
params.relativeErrorTol = 1e-9;
Values actual = *LevenbergMarquardtOptimizer(graph, init, params).optimized();
EXPECT(assert_equal(expected, actual, 1e-4));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

8
gtsam/slam/tests/testVSLAM.cpp
View File

@ -23,7 +23,7 @@
using namespace boost; using namespace boost;
#include <gtsam/nonlinear/NonlinearFactorGraph.h> #include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h> #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/visualSLAM.h> #include <gtsam/slam/visualSLAM.h>
using namespace std; using namespace std;
@ -102,7 +102,7 @@ TEST( Graph, optimizeLM)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering)); NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
@ -139,7 +139,7 @@ TEST( Graph, optimizeLM2)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate, ordering)); NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate, ordering));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started
@ -172,7 +172,7 @@ TEST( Graph, CHECK_ORDERING)
// Create an optimizer and check its error // Create an optimizer and check its error
// We expect the initial to be zero because config is the ground truth // We expect the initial to be zero because config is the ground truth
NonlinearOptimizer::auto_ptr optimizer(new GaussNewtonOptimizer(graph, initialEstimate)); NonlinearOptimizer::auto_ptr optimizer(new LevenbergMarquardtOptimizer(graph, initialEstimate));
DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9); DOUBLES_EQUAL(0.0, optimizer->error(), 1e-9);
// Iterate once, and the config should not have changed because we started // Iterate once, and the config should not have changed because we started