added extra unit test
parent
282aa1a0a9
commit
038c1c0b8e
|
@ -306,7 +306,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor {
|
|||
case HESSIAN:
|
||||
return createHessianFactor(values, lambda);
|
||||
default:
|
||||
throw std::runtime_error("SmartStereoFactorlinearize: unknown mode");
|
||||
throw std::runtime_error("SmartStereoProjectionFactorPP: unknown linearization mode");
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
@ -862,6 +862,103 @@ TEST( SmartStereoProjectionFactorPP, 3poses_optimization_sameExtrinsicKey ) {
|
|||
EXPECT(assert_equal(expected, delta, 1e-4));
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionFactorPP, 3poses_optimization_2ExtrinsicKeys ) {
|
||||
|
||||
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
|
||||
Pose3 w_Pose_cam1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(0, 0, 1));
|
||||
StereoCamera cam1(w_Pose_cam1, K2);
|
||||
|
||||
// create second camera 1 meter to the right of first camera
|
||||
Pose3 w_Pose_cam2 = w_Pose_cam1 * Pose3(Rot3(), Point3(1, 0, 0));
|
||||
StereoCamera cam2(w_Pose_cam2, K2);
|
||||
|
||||
// create third camera 1 meter above the first camera
|
||||
Pose3 w_Pose_cam3 = w_Pose_cam1 * Pose3(Rot3(), Point3(0, -1, 0));
|
||||
StereoCamera cam3(w_Pose_cam3, K2);
|
||||
|
||||
// three landmarks ~5 meters infront of camera
|
||||
Point3 landmark1(5, 0.5, 1.2);
|
||||
Point3 landmark2(5, -0.5, 1.2);
|
||||
Point3 landmark3(3, 0, 3.0);
|
||||
|
||||
// 1. Project three landmarks into three cameras and triangulate
|
||||
vector<StereoPoint2> measurements_l1 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark1);
|
||||
vector<StereoPoint2> measurements_l2 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark2);
|
||||
vector<StereoPoint2> measurements_l3 = stereo_projectToMultipleCameras(cam1,
|
||||
cam2, cam3, landmark3);
|
||||
|
||||
KeyVector poseKeys;
|
||||
poseKeys.push_back(x1);
|
||||
poseKeys.push_back(x2);
|
||||
poseKeys.push_back(x3);
|
||||
|
||||
KeyVector extrinsicKeys;
|
||||
extrinsicKeys.push_back(body_P_cam1_key);
|
||||
extrinsicKeys.push_back(body_P_cam1_key);
|
||||
extrinsicKeys.push_back(body_P_cam3_key);
|
||||
|
||||
SmartStereoProjectionParams smart_params;
|
||||
smart_params.triangulation.enableEPI = true;
|
||||
SmartStereoProjectionFactorPP::shared_ptr smartFactor1(new SmartStereoProjectionFactorPP(model, smart_params));
|
||||
smartFactor1->add(measurements_l1, poseKeys, extrinsicKeys, K2);
|
||||
|
||||
SmartStereoProjectionFactorPP::shared_ptr smartFactor2(new SmartStereoProjectionFactorPP(model, smart_params));
|
||||
smartFactor2->add(measurements_l2, poseKeys, extrinsicKeys, K2);
|
||||
|
||||
SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params));
|
||||
smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2);
|
||||
|
||||
// relevant poses:
|
||||
Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0));
|
||||
Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse());
|
||||
Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse());
|
||||
Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse());
|
||||
|
||||
// Graph
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, w_Pose_body1, noisePrior);
|
||||
graph.addPrior(x2, w_Pose_body2, noisePrior);
|
||||
graph.addPrior(x3, w_Pose_body3, noisePrior);
|
||||
// graph.addPrior(body_P_cam1_key, body_Pose_cam, noisePrior);
|
||||
// we might need some prior on the calibration too
|
||||
// graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this!
|
||||
|
||||
// Values
|
||||
Values values;
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise
|
||||
values.insert(x1, w_Pose_body1);
|
||||
values.insert(x2, w_Pose_body2);
|
||||
values.insert(x3, w_Pose_body3);
|
||||
values.insert(body_P_cam1_key, body_Pose_cam*noise_pose);
|
||||
values.insert(body_P_cam3_key, body_Pose_cam*noise_pose);
|
||||
|
||||
// cost is large before optimization
|
||||
double initialErrorSmart = graph.error(values);
|
||||
EXPECT_DOUBLES_EQUAL(31986.961831653316, initialErrorSmart, 1e-5); // initial guess is noisy, so nonzero error
|
||||
|
||||
Values result;
|
||||
gttic_(SmartStereoProjectionFactorPP);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lm_params);
|
||||
result = optimizer.optimize();
|
||||
gttoc_(SmartStereoProjectionFactorPP);
|
||||
tictoc_finishedIteration_();
|
||||
|
||||
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5);
|
||||
|
||||
// NOTE: the following would fail since the problem is underconstrained (while LM above works just fine!)
|
||||
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(result);
|
||||
// VectorValues delta = GFG->optimize();
|
||||
// VectorValues expected = VectorValues::Zero(delta);
|
||||
// EXPECT(assert_equal(expected, delta, 1e-4));
|
||||
}
|
||||
|
||||
/* *************************************************************************/
|
||||
TEST( SmartStereoProjectionFactorPP, monocular_multipleExtrinsicKeys ){
|
||||
// make a realistic calibration matrix
|
||||
|
|
Loading…
Reference in New Issue