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

adding 3 camera, 3 landmark test

release/4.3a0
Chris Beall 2013-08-05 15:50:19 +00:00
parent 39ec641c4a
commit d1de6b29a9
1 changed files with 55 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

95
gtsam_unstable/slam/tests/testSmartProjectionFactor.cpp
View File

@ -151,16 +151,6 @@ TEST( MultiProjectionFactor, noisy ){
double actualError = smartFactor->error(values); double actualError = smartFactor->error(values);
double expectedError = sqrt(0.08); double expectedError = sqrt(0.08);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor);
graph.add(PriorFactor<Pose3>(x1, level_pose, noisePrior));
LevenbergMarquardtOptimizer optimizer(graph, values);
Values result = optimizer.optimize();
result.print("results of the optimization \n");
// we do not expect to be able to predict the error, since the error on the pixel will change // we do not expect to be able to predict the error, since the error on the pixel will change
// the triangulation of the landmark which is internal to the factor. // the triangulation of the landmark which is internal to the factor.
// DOUBLES_EQUAL(expectedError, actualError, 1e-7); // DOUBLES_EQUAL(expectedError, actualError, 1e-7);
@ -169,7 +159,7 @@ TEST( MultiProjectionFactor, noisy ){
///* ************************************************************************* */ ///* ************************************************************************* */
TEST( MultiProjectionFactor, 3poses ){ TEST( MultiProjectionFactor, 3poses ){
cout << " ************************ MultiProjectionFactor: noisy ****************************" << endl; cout << " ************************ MultiProjectionFactor: 3 cams + 3 landmarks **********************" << endl;
Symbol x1('X', 1); Symbol x1('X', 1);
Symbol x2('X', 2); Symbol x2('X', 2);
@ -178,53 +168,78 @@ TEST( MultiProjectionFactor, 3poses ){
const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1); const SharedDiagonal noiseProjection = noiseModel::Isotropic::Sigma(2, 1);
std::vector<Key> views; std::vector<Key> views;
views += x1, x2; //, x3; views += x1, x2, x3;
Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480)); Cal3_S2::shared_ptr K(new Cal3_S2(1500, 1200, 0, 640, 480));
// 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 level_pose = 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));
SimpleCamera level_camera(level_pose, *K); SimpleCamera 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 level_pose_right = level_pose * Pose3(Rot3(), Point3(1,0,0)); Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera level_camera_right(level_pose_right, *K); SimpleCamera cam2(pose2, *K);
// landmark ~5 meters infront of camera // create third camera 1 meter above the first camera
Point3 landmark(5, 0.5, 1.2); Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam3(pose2, *K);
// 1. Project two landmarks into two cameras and triangulate // three landmarks ~5 meters infront of camera
Point2 pixelError(0.2,0.2); Point3 landmark1(5, 0.5, 1.2);
Point2 level_uv = level_camera.project(landmark) + pixelError; Point3 landmark2(5, -0.5, 1.2);
Point2 level_uv_right = level_camera_right.project(landmark); Point3 landmark3(5, 0, 3.0);
Values values; vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
values.insert(x1, level_pose);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
values.insert(x2, level_pose_right.compose(noise_pose));
// poses += level_pose, level_pose_right; // 1. Project three landmarks into three cameras and triangulate
vector<Point2> measurements; Point2 cam1_uv1 = cam1.project(landmark1);
measurements += level_uv, level_uv_right; Point2 cam2_uv1 = cam2.project(landmark1);
Point2 cam3_uv1 = cam3.project(landmark1);
measurements_cam1 += cam1_uv1, cam2_uv1, cam3_uv1;
SmartProjectionFactor<Pose3, Point3, Cal3_S2>::shared_ptr //
smartFactor(new SmartProjectionFactor<Pose3, Point3, Cal3_S2>(measurements, noiseProjection, views, K)); Point2 cam1_uv2 = cam1.project(landmark2);
Point2 cam2_uv2 = cam2.project(landmark2);
Point2 cam3_uv2 = cam3.project(landmark2);
measurements_cam2 += cam1_uv2, cam2_uv2, cam3_uv2;
double actualError = smartFactor->error(values);
double expectedError = sqrt(0.08); Point2 cam1_uv3 = cam1.project(landmark3);
Point2 cam2_uv3 = cam2.project(landmark3);
Point2 cam3_uv3 = cam3.project(landmark3);
measurements_cam3 += cam1_uv3, cam2_uv3, cam3_uv3;
typedef SmartProjectionFactor<Pose3, Point3, Cal3_S2> SmartFactor;
SmartFactor::shared_ptr smartFactor1(new SmartFactor(measurements_cam1, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor2(new SmartFactor(measurements_cam2, noiseProjection, views, K));
SmartFactor::shared_ptr smartFactor3(new SmartFactor(measurements_cam3, noiseProjection, views, K));
// double actualError = smartFactor->error(values);
// double expectedError = sqrt(0.08);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph; NonlinearFactorGraph graph;
graph.push_back(smartFactor); graph.push_back(smartFactor1);
graph.add(PriorFactor<Pose3>(x1, level_pose, noisePrior)); graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.add(PriorFactor<Pose3>(x1, pose1, noisePrior));
LevenbergMarquardtOptimizer optimizer(graph, values);
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3));
Values values;
values.insert(x1, pose1);
values.insert(x2, pose1);
values.insert(x3, pose3* noise_pose);
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
result.print("results of the optimization \n"); result.print("results of 3 camera, 3 landmark optimization \n");
// we do not expect to be able to predict the error, since the error on the pixel will change
// the triangulation of the landmark which is internal to the factor.
// DOUBLES_EQUAL(expectedError, actualError, 1e-7);
} }