Cleaned up test

release/4.3a0
dellaert 2015-03-05 10:41:49 -08:00
parent b40c0f7f15
commit 758aab6e80
1 changed files with 6 additions and 68 deletions

View File

@ -18,8 +18,8 @@
* @date Sept 2013
*/
// TODO #include <gtsam/slam/tests/smartFactorScenarios.h>
#include <gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PoseTranslationPrior.h>
#include <gtsam/slam/ProjectionFactor.h>
@ -32,8 +32,6 @@ using namespace std;
using namespace boost::assign;
using namespace gtsam;
static bool isDebugTest = false;
// make a realistic calibration matrix
static double fov = 60; // degrees
static size_t w = 640, h = 480;
@ -83,9 +81,10 @@ vector<StereoPoint2> stereo_projectToMultipleCameras(const StereoCamera& cam1,
return measurements_cam;
}
LevenbergMarquardtParams params;
/* ************************************************************************* */
TEST( SmartStereoProjectionPoseFactor, Constructor) {
fprintf(stderr, "Test 1 Complete");
SmartFactor::shared_ptr factor1(new SmartFactor());
}
@ -119,8 +118,6 @@ TEST( SmartStereoProjectionPoseFactor, Equals ) {
/* *************************************************************************/
TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
@ -160,8 +157,6 @@ TEST_UNSAFE( SmartStereoProjectionPoseFactor, noiseless ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, noisy ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: noisy ****************************" << endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 level_pose = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2),
Point3(0, 0, 1));
@ -217,10 +212,6 @@ TEST( SmartStereoProjectionPoseFactor, noisy ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
cout
<< " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks **********************"
<< endl;
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
StereoCamera cam1(pose1, K2);
@ -277,8 +268,6 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
values.insert(x2, pose2);
// initialize third pose with some noise, we expect it to move back to original pose3
values.insert(x3, pose3 * noise_pose);
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
EXPECT(
assert_equal(
Pose3(
@ -288,14 +277,8 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
EXPECT_DOUBLES_EQUAL(1888864, graph.error(values), 1);
LevenbergMarquardtParams params;
if (isDebugTest)
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
if (isDebugTest)
params.verbosity = NonlinearOptimizerParams::ERROR;
Values result;
gttic_ (SmartStereoProjectionPoseFactor);
gttic_(SmartStereoProjectionPoseFactor);
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
gttoc_(SmartStereoProjectionPoseFactor);
@ -306,14 +289,10 @@ TEST( SmartStereoProjectionPoseFactor, 3poses_smart_projection_factor ) {
GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
VectorValues delta = GFG->optimize();
VectorValues expected = VectorValues::Zero(delta);
EXPECT(assert_equal(expected, delta,1e-6));
EXPECT(assert_equal(expected, delta, 1e-6));
// result.print("results of 3 camera, 3 landmark optimization \n");
if (isDebugTest)
result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
if (isDebugTest)
tictoc_print_();
}
/* *************************************************************************/
@ -376,7 +355,6 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) {
values.insert(x2, pose2);
values.insert(x3, pose3 * noise_pose);
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
@ -449,7 +427,6 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) {
values.insert(x3, pose3 * noise_pose);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
@ -533,7 +510,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
values.insert(x3, pose3);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
@ -595,8 +571,7 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(x2, pose2);
// values.insert(x3, pose3*noise_pose);
//
// LevenbergMarquardtParams params;
// Values result;
//// Values result;
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// result = optimizer.optimize();
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
@ -604,7 +579,6 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_projection_factor ){
// // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -656,15 +630,10 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) {
// values.insert(L(1), landmark1);
// values.insert(L(2), landmark2);
// values.insert(L(3), landmark3);
// if(isDebugTest) values.at<Pose3>(x3).print("Pose3 before optimization: ");
//
// LevenbergMarquardtParams params;
// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
// LevenbergMarquardtOptimizer optimizer(graph, values, params);
// Values result = optimizer.optimize();
//
// if(isDebugTest) result.at<Pose3>(x3).print("Pose3 after optimization: ");
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
//}
//
@ -726,8 +695,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100),
Point3(0.1, 0.1, 0.1)); // smaller noise
values.insert(x3, pose3 * noise_pose);
if (isDebugTest)
values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
// TODO: next line throws Cheirality exception on Mac
boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(
@ -752,7 +719,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
+ hessianFactor3->augmentedInformation();
// Check Information vector
// cout << AugInformationMatrix.size() << endl;
Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
// Check Hessian
@ -761,7 +727,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 2 landmarks: Rotation Only**********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -814,11 +779,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// values.insert(x2, pose2*noise_pose);
// // initialize third pose with some noise, we expect it to move back to original pose3
// values.insert(x3, pose3*noise_pose*noise_pose);
// if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
//
// LevenbergMarquardtParams params;
// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
@ -828,15 +788,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// tictoc_finishedIteration_();
//
// // result.print("results of 3 camera, 3 landmark optimization \n");
// if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// if(isDebugTest) tictoc_print_();
//}
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: 3 cams + 3 landmarks: Rotation Only**********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -897,11 +853,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// values.insert(x2, pose2);
// // initialize third pose with some noise, we expect it to move back to original pose3
// values.insert(x3, pose3*noise_pose);
// if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
//
// LevenbergMarquardtParams params;
// if(isDebugTest) params.verbosityLM = LevenbergMarquardtParams::TRYDELTA;
// if(isDebugTest) params.verbosity = NonlinearOptimizerParams::ERROR;
//
// Values result;
// gttic_(SmartStereoProjectionPoseFactor);
@ -911,15 +862,11 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// tictoc_finishedIteration_();
//
// // result.print("results of 3 camera, 3 landmark optimization \n");
// if(isDebugTest) result.at<Pose3>(x3).print("Smart: Pose3 after optimization: ");
// cout << "TEST COMMENTED: rotation only version of smart factors has been deprecated " << endl;
// // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
// if(isDebugTest) tictoc_print_();
//}
//
///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, Hessian ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: Hessian **********************" << endl;
//
// vector<Key> views;
// views.push_back(x1);
@ -952,7 +899,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
// values.insert(x2, pose2);
//
// boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor1->linearize(values);
// if(isDebugTest) hessianFactor->print("Hessian factor \n");
//
// // compute triangulation from linearization point
// // compute reprojection errors (sum squared)
@ -963,8 +909,6 @@ TEST( SmartStereoProjectionPoseFactor, CheckHessian) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl;
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -1034,8 +978,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ) {
/* *************************************************************************/
TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
// cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl;
vector<Key> views;
views.push_back(x1);
views.push_back(x2);
@ -1066,8 +1008,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(
values);
if (isDebugTest)
hessianFactor->print("Hessian factor \n");
Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 0));
@ -1078,8 +1018,6 @@ TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ) {
boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(
rotValues);
if (isDebugTest)
hessianFactorRot->print("Hessian factor \n");
// Hessian is invariant to rotations in the nondegenerate case
EXPECT(