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

Hessian tests work, too

release/4.3a0
cbeall3 2014-07-22 22:02:20 -04:00
parent f75f26fb63
commit cbad9aa783
1 changed files with 201 additions and 208 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

409
gtsam/slam/tests/testSmartStereoProjectionPoseFactor.cpp
View File

@ -626,85 +626,84 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
// EXPECT(assert_equal(pose3,result.at<Pose3>(x3))); // EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
//} //}
// //
///* *************************************************************************/ /* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, CheckHessian){ TEST( SmartStereoProjectionPoseFactor, CheckHessian){
//
// std::vector<Key> views; std::vector<Key> views;
// views.push_back(x1); views.push_back(x1);
// views.push_back(x2); views.push_back(x2);
// views.push_back(x3); views.push_back(x3);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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), gtsam::Point3(0,0,1)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// StereoCamera cam1(pose1, K); StereoCamera cam1(pose1, K);
//
// // create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
// Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
// StereoCamera cam2(pose2, K); StereoCamera cam2(pose2, K);
//
// // create third camera 1 meter above the first camera // create third camera 1 meter above the first camera
// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0,0,0));
// StereoCamera cam3(pose3, K); StereoCamera cam3(pose3, K);
//
// // three landmarks ~5 meters infront of camera // three landmarks ~5 meters infront of camera
// Point3 landmark1(5, 0.5, 1.2); Point3 landmark1(5, 0.5, 1.2);
// Point3 landmark2(5, -0.5, 1.2); Point3 landmark2(5, -0.5, 1.2);
// Point3 landmark3(3, 0, 3.0); Point3 landmark3(3, 0, 3.0);
//
// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3; // 1. Project three landmarks into three cameras and triangulate
// vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
// // 1. Project three landmarks into three cameras and triangulate vector<StereoPoint2> measurements_cam2 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); vector<StereoPoint2> measurements_cam3 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// double rankTol = 10;
// double rankTol = 10;
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol));
// SmartFactor::shared_ptr smartFactor1(new SmartFactor(rankTol)); smartFactor1->add(measurements_cam1, views, model, K);
// smartFactor1->add(measurements_cam1, views, model, K);
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol));
// SmartFactor::shared_ptr smartFactor2(new SmartFactor(rankTol)); smartFactor2->add(measurements_cam2, views, model, K);
// smartFactor2->add(measurements_cam2, views, model, K);
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol));
// SmartFactor::shared_ptr smartFactor3(new SmartFactor(rankTol)); smartFactor3->add(measurements_cam3, views, model, K);
// smartFactor3->add(measurements_cam3, views, model, K);
// NonlinearFactorGraph graph;
// NonlinearFactorGraph graph; graph.push_back(smartFactor1);
// graph.push_back(smartFactor1); graph.push_back(smartFactor2);
// graph.push_back(smartFactor2); graph.push_back(smartFactor3);
// graph.push_back(smartFactor3);
// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
// // Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise Values values;
// Values values; values.insert(x1, pose1);
// values.insert(x1, pose1); values.insert(x2, pose2);
// values.insert(x2, pose2); // initialize third pose with some noise, we expect it to move back to original pose3
// // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, pose3*noise_pose);
// values.insert(x3, pose3*noise_pose); if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
// if(isDebugTest) values.at<Pose3>(x3).print("Smart: Pose3 before optimization: ");
// boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(values);
// boost::shared_ptr<GaussianFactor> hessianFactor1 = smartFactor1->linearize(values); boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(values);
// boost::shared_ptr<GaussianFactor> hessianFactor2 = smartFactor2->linearize(values); boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(values);
// boost::shared_ptr<GaussianFactor> hessianFactor3 = smartFactor3->linearize(values);
// Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information();
// Matrix CumulativeInformation = hessianFactor1->information() + hessianFactor2->information() + hessianFactor3->information();
// boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(values);
// boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(values); Matrix GraphInformation = GaussianGraph->hessian().first;
// Matrix GraphInformation = GaussianGraph->hessian().first;
// // Check Hessian
// // Check Hessian EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8));
// EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-8));
// Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() +
// Matrix AugInformationMatrix = hessianFactor1->augmentedInformation() + hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation();
// hessianFactor2->augmentedInformation() + hessianFactor3->augmentedInformation();
// // Check Information vector
// // Check Information vector // cout << AugInformationMatrix.size() << endl;
// // cout << AugInformationMatrix.size() << endl; Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector
// Vector InfoVector = AugInformationMatrix.block(0,18,18,1); // 18x18 Hessian + information vector
// // Check Hessian
// // Check Hessian EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8));
// EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-8)); }
//}
// //
///* *************************************************************************/ ///* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){ //TEST( SmartStereoProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_factor ){
@ -907,135 +906,129 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ){
// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] // // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
//} //}
// //
//
///* *************************************************************************/ /* *************************************************************************/
//TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){ TEST( SmartStereoProjectionPoseFactor, HessianWithRotation ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl; // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian **********************" << endl;
//
// std::vector<Key> views; std::vector<Key> views;
// views.push_back(x1); views.push_back(x1);
// views.push_back(x2); views.push_back(x2);
// views.push_back(x3); views.push_back(x3);
//
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) // 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), gtsam::Point3(0,0,1)); Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// StereoCamera cam1(pose1, K); StereoCamera cam1(pose1, K);
//
// // create second camera 1 meter to the right of first camera // create second camera 1 meter to the right of first camera
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
// StereoCamera cam2(pose2, K); StereoCamera cam2(pose2, K);
//
// // create third camera 1 meter above the first camera // create third camera 1 meter above the first camera
// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0)); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
// StereoCamera cam3(pose3, K); StereoCamera cam3(pose3, K);
//
// Point3 landmark1(5, 0.5, 1.2); Point3 landmark1(5, 0.5, 1.2);
//
// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3; vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
//
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
// smartFactorInstance->add(measurements_cam1, views, model, K);
// SmartFactor::shared_ptr smartFactorInstance(new SmartFactor());
// smartFactorInstance->add(measurements_cam1, views, model, K); Values values;
// values.insert(x1, pose1);
// Values values; values.insert(x2, pose2);
// values.insert(x1, pose1); values.insert(x3, pose3);
// values.insert(x2, pose2);
// values.insert(x3, pose3); boost::shared_ptr<GaussianFactor> hessianFactor = smartFactorInstance->linearize(values);
// // hessianFactor->print("Hessian factor \n");
// boost::shared_ptr<GaussianFactor> hessianFactor = smartFactorInstance->linearize(values);
// // hessianFactor->print("Hessian factor \n"); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));
//
// Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0)); Values rotValues;
// rotValues.insert(x1, poseDrift.compose(pose1));
// Values rotValues; rotValues.insert(x2, poseDrift.compose(pose2));
// rotValues.insert(x1, poseDrift.compose(pose1)); rotValues.insert(x3, poseDrift.compose(pose3));
// rotValues.insert(x2, poseDrift.compose(pose2));
// rotValues.insert(x3, poseDrift.compose(pose3)); boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactorInstance->linearize(rotValues);
// // hessianFactorRot->print("Hessian factor \n");
// boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactorInstance->linearize(rotValues);
// // hessianFactorRot->print("Hessian factor \n"); // Hessian is invariant to rotations in the nondegenerate case
// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );
// // Hessian is invariant to rotations in the nondegenerate case
// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));
//
// Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5)); Values tranValues;
// tranValues.insert(x1, poseDrift2.compose(pose1));
// Values tranValues; tranValues.insert(x2, poseDrift2.compose(pose2));
// tranValues.insert(x1, poseDrift2.compose(pose1)); tranValues.insert(x3, poseDrift2.compose(pose3));
// tranValues.insert(x2, poseDrift2.compose(pose2));
// tranValues.insert(x3, poseDrift2.compose(pose3)); boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactorInstance->linearize(tranValues);
//
// boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactorInstance->linearize(tranValues); // Hessian is invariant to rotations and translations in the nondegenerate case
// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
// // Hessian is invariant to rotations and translations in the nondegenerate case }
// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
//} /* *************************************************************************/
// TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){
///* *************************************************************************/ // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl;
//TEST( SmartStereoProjectionPoseFactor, HessianWithRotationDegenerate ){
// // cout << " ************************ SmartStereoProjectionPoseFactor: rotated Hessian (degenerate) **********************" << endl; std::vector<Key> views;
// views.push_back(x1);
// std::vector<Key> views; views.push_back(x2);
// views.push_back(x1); views.push_back(x3);
// views.push_back(x2);
// views.push_back(x3); // 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), gtsam::Point3(0,0,1));
// // create first camera. Looking along X-axis, 1 meter above ground plane (x-y) StereoCamera cam1(pose1, K2);
// Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
// StereoCamera cam1(pose1, K2); // Second and third cameras in same place, which is a degenerate configuration
// Pose3 pose2 = pose1;
// // create second camera 1 meter to the right of first camera Pose3 pose3 = pose1;
// Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(0,0,0)); StereoCamera cam2(pose2, K2);
// StereoCamera cam2(pose2, K2); StereoCamera cam3(pose3, K2);
//
// // create third camera 1 meter above the first camera Point3 landmark1(5, 0.5, 1.2);
// Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,0,0));
// StereoCamera cam3(pose3, K2); vector<StereoPoint2> measurements_cam1 = stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1);
//
// Point3 landmark1(5, 0.5, 1.2); SmartFactor::shared_ptr smartFactor(new SmartFactor());
// smartFactor->add(measurements_cam1, views, model, K2);
// vector<StereoPoint2> measurements_cam1, measurements_cam2, measurements_cam3;
//
// stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); Values values;
// values.insert(x1, pose1);
// SmartFactor::shared_ptr smartFactor(new SmartFactor()); values.insert(x2, pose2);
// smartFactor->add(measurements_cam1, views, model, K2); values.insert(x3, pose3);
//
// boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(values);
// Values values; if(isDebugTest) hessianFactor->print("Hessian factor \n");
// values.insert(x1, pose1);
// values.insert(x2, pose2); Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));
// values.insert(x3, pose3);
// Values rotValues;
// boost::shared_ptr<GaussianFactor> hessianFactor = smartFactor->linearize(values); rotValues.insert(x1, poseDrift.compose(pose1));
// if(isDebugTest) hessianFactor->print("Hessian factor \n"); rotValues.insert(x2, poseDrift.compose(pose2));
// rotValues.insert(x3, poseDrift.compose(pose3));
// Pose3 poseDrift = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,0));
// boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(rotValues);
// Values rotValues; if(isDebugTest) hessianFactorRot->print("Hessian factor \n");
// rotValues.insert(x1, poseDrift.compose(pose1));
// rotValues.insert(x2, poseDrift.compose(pose2)); // Hessian is invariant to rotations in the nondegenerate case
// rotValues.insert(x3, poseDrift.compose(pose3)); EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) );
//
// boost::shared_ptr<GaussianFactor> hessianFactorRot = smartFactor->linearize(rotValues); Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));
// if(isDebugTest) hessianFactorRot->print("Hessian factor \n");
// Values tranValues;
// // Hessian is invariant to rotations in the nondegenerate case tranValues.insert(x1, poseDrift2.compose(pose1));
// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRot->information(), 1e-8) ); tranValues.insert(x2, poseDrift2.compose(pose2));
// tranValues.insert(x3, poseDrift2.compose(pose3));
// Pose3 poseDrift2 = Pose3(Rot3::ypr(-M_PI/2, -M_PI/3, -M_PI/2), gtsam::Point3(10,-4,5));
// boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactor->linearize(tranValues);
// Values tranValues;
// tranValues.insert(x1, poseDrift2.compose(pose1)); // Hessian is invariant to rotations and translations in the nondegenerate case
// tranValues.insert(x2, poseDrift2.compose(pose2)); EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
// tranValues.insert(x3, poseDrift2.compose(pose3)); }
//
// boost::shared_ptr<GaussianFactor> hessianFactorRotTran = smartFactor->linearize(tranValues);
//
// // Hessian is invariant to rotations and translations in the nondegenerate case
// EXPECT(assert_equal(hessianFactor->information(), hessianFactorRotTran->information(), 1e-8) );
//}
/* ************************************************************************* */ /* ************************************************************************* */