fixed test with lmk distance
lcarlone
parent
934413522d
commit
aeb1d35dd6
|
|
@ -456,121 +456,71 @@ TEST( SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses ) {
|
|||
EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6);
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) {
|
||||
std::cout << "===================" << std::endl;
|
||||
using namespace vanillaPose;
|
||||
/* *************************************************************************/
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
|
||||
using namespace vanillaPoseRS;
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
KeyVector views {x1, x2, x3};
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
// create inputs
|
||||
std::vector<std::pair<Key, Key>> key_pairs;
|
||||
key_pairs.push_back(std::make_pair(x1, x2));
|
||||
key_pairs.push_back(std::make_pair(x2, x3));
|
||||
key_pairs.push_back(std::make_pair(x3, x1));
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
std::vector<double> interp_factors;
|
||||
interp_factors.push_back(interp_factor1);
|
||||
interp_factors.push_back(interp_factor2);
|
||||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(new SmartFactor(model, sharedK));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
double excludeLandmarksFutherThanDist = 2;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(1.0);
|
||||
params.setLinearizationMode(gtsam::HESSIAN);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setEnableEPI(false);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(new SmartFactor(model, sharedK));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
SmartFactorRS smartFactor1(model,params);
|
||||
smartFactor1.add(measurements_cam1, key_pairs, interp_factors, sharedK);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(new SmartFactor(model, sharedK));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
SmartFactorRS smartFactor2(model,params);
|
||||
smartFactor2.add(measurements_cam2, key_pairs, interp_factors, sharedK);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
SmartFactorRS smartFactor3(model,params);
|
||||
smartFactor3.add(measurements_cam3, key_pairs, interp_factors, sharedK);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, cam1.pose(), noisePrior);
|
||||
graph.addPrior(x2, cam2.pose(), noisePrior);
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), 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),
|
||||
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, cam1.pose());
|
||||
values.insert(x2, cam2.pose());
|
||||
// initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
EXPECT(
|
||||
assert_equal(
|
||||
Pose3(
|
||||
Rot3(1.11022302e-16, -0.0314107591, 0.99950656, -0.99950656,
|
||||
-0.0313952598, -0.000986635786, 0.0314107591, -0.999013364,
|
||||
-0.0313952598), Point3(0.1, -0.1, 1.9)),
|
||||
values.at<Pose3>(x3)));
|
||||
NonlinearFactorGraph graph;
|
||||
graph.push_back(smartFactor1);
|
||||
graph.push_back(smartFactor2);
|
||||
graph.push_back(smartFactor3);
|
||||
graph.addPrior(x1, level_pose, noisePrior);
|
||||
graph.addPrior(x2, pose_right, noisePrior);
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
|
||||
}
|
||||
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, level_pose);
|
||||
values.insert(x2, pose_right);
|
||||
// initialize third pose with some noise, we expect it to move back to original pose_above
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) {
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
double excludeLandmarksFutherThanDist = 2;
|
||||
|
||||
KeyVector views {x1, x2, x3};
|
||||
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(1.0);
|
||||
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setEnableEPI(false);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(model, sharedK, params));
|
||||
smartFactor1->add(measurements_cam1, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(model, sharedK, params));
|
||||
smartFactor2->add(measurements_cam2, views);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(model, sharedK, params));
|
||||
smartFactor3->add(measurements_cam3, views);
|
||||
|
||||
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, cam1.pose(), noisePrior);
|
||||
graph.addPrior(x2, cam2.pose(), noisePrior);
|
||||
|
||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI/10, 0., -M_PI/10), 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),
|
||||
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||
Values values;
|
||||
values.insert(x1, cam1.pose());
|
||||
values.insert(x2, cam2.pose());
|
||||
values.insert(x3, pose_above * noise_pose);
|
||||
|
||||
// All factors are disabled and pose should remain where it is
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
|
||||
}
|
||||
// All factors are disabled and pose should remain where it is
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
|
||||
}
|
||||
|
||||
/* *************************************************************************
|
||||
TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) {
|
||||
std::cout << "===================" << std::endl;
|
||||
|
||||
using namespace vanillaPose;
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue