Cleaned up test
dellaert
parent
b40c0f7f15
commit
758aab6e80
|
|
@ -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(
|
||||
|
|
|
|||
Loading…
Reference in New Issue