working on tests
lcarlone
parent
eb82878044
commit
fa4de18742
|
|
@ -127,8 +127,10 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
||||||
*/
|
*/
|
||||||
void add(const Point2Vector& measurements, const KeyVector& poseKeys,
|
void add(const Point2Vector& measurements, const KeyVector& poseKeys,
|
||||||
const FastVector<size_t>& cameraIds) {
|
const FastVector<size_t>& cameraIds) {
|
||||||
assert(poseKeys.size() == measurements.size());
|
if(poseKeys.size() != measurements.size() || poseKeys.size() != cameraIds.size()){
|
||||||
assert(poseKeys.size() == cameraIds.size());
|
throw std::runtime_error("SmartProjectionRigFactor: "
|
||||||
|
"trying to add inconsistent inputs");
|
||||||
|
}
|
||||||
for (size_t i = 0; i < measurements.size(); i++) {
|
for (size_t i = 0; i < measurements.size(); i++) {
|
||||||
add(measurements[i], poseKeys[i], cameraIds[i]);
|
add(measurements[i], poseKeys[i], cameraIds[i]);
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -506,255 +506,252 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) {
|
||||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
|
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
|
||||||
}
|
}
|
||||||
|
|
||||||
///* *************************************************************************/
|
/* *************************************************************************/
|
||||||
//TEST( SmartProjectionRigFactor, landmarkDistance ) {
|
TEST( SmartProjectionRigFactor, landmarkDistance ) {
|
||||||
//
|
|
||||||
// using namespace vanillaPose;
|
using namespace vanillaPose;
|
||||||
//
|
|
||||||
// double excludeLandmarksFutherThanDist = 2;
|
double excludeLandmarksFutherThanDist = 2;
|
||||||
//
|
|
||||||
// KeyVector views { x1, x2, x3 };
|
KeyVector views { x1, x2, x3 };
|
||||||
//
|
|
||||||
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||||
//
|
|
||||||
// // Project three landmarks into three cameras
|
// Project three landmarks into three cameras
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||||
//
|
|
||||||
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
|
SmartProjectionParams params;
|
||||||
// sharedKs.push_back(sharedK);
|
params.setRankTolerance(1.0);
|
||||||
// sharedKs.push_back(sharedK);
|
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
||||||
// sharedKs.push_back(sharedK);
|
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||||
//
|
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||||
// SmartProjectionParams params;
|
params.setEnableEPI(false);
|
||||||
// params.setRankTolerance(1.0);
|
|
||||||
// params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
Cameras cameraRig; // single camera in the rig
|
||||||
// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||||
// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
FastVector<size_t> cameraIds { 0, 0, 0};
|
||||||
// params.setEnableEPI(false);
|
|
||||||
//
|
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params));
|
||||||
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params));
|
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||||
// smartFactor1->add(measurements_cam1, views, sharedKs);
|
|
||||||
//
|
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params));
|
||||||
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params));
|
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||||
// smartFactor2->add(measurements_cam2, views, sharedKs);
|
|
||||||
//
|
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params));
|
||||||
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params));
|
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||||
// smartFactor3->add(measurements_cam3, views, sharedKs);
|
|
||||||
//
|
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(smartFactor1);
|
||||||
// graph.push_back(smartFactor1);
|
graph.push_back(smartFactor2);
|
||||||
// graph.push_back(smartFactor2);
|
graph.push_back(smartFactor3);
|
||||||
// graph.push_back(smartFactor3);
|
graph.addPrior(x1, cam1.pose(), noisePrior);
|
||||||
// graph.addPrior(x1, cam1.pose(), noisePrior);
|
graph.addPrior(x2, cam2.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/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),
|
||||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||||
// Point3(0.1, 0.1, 0.1)); // smaller noise
|
Values values;
|
||||||
// Values values;
|
values.insert(x1, cam1.pose());
|
||||||
// values.insert(x1, cam1.pose());
|
values.insert(x2, cam2.pose());
|
||||||
// values.insert(x2, cam2.pose());
|
values.insert(x3, pose_above * noise_pose);
|
||||||
// values.insert(x3, pose_above * noise_pose);
|
|
||||||
//
|
// All factors are disabled and pose should remain where it is
|
||||||
// // All factors are disabled and pose should remain where it is
|
Values result;
|
||||||
// Values result;
|
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
result = optimizer.optimize();
|
||||||
// result = optimizer.optimize();
|
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
|
||||||
// EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
|
}
|
||||||
//}
|
|
||||||
//
|
/* *************************************************************************/
|
||||||
///* *************************************************************************/
|
TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
|
||||||
//TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) {
|
|
||||||
//
|
using namespace vanillaPose;
|
||||||
// using namespace vanillaPose;
|
|
||||||
//
|
double excludeLandmarksFutherThanDist = 1e10;
|
||||||
// double excludeLandmarksFutherThanDist = 1e10;
|
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
|
||||||
// double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
|
|
||||||
//
|
KeyVector views { x1, x2, x3 };
|
||||||
// KeyVector views { x1, x2, x3 };
|
|
||||||
//
|
// add fourth landmark
|
||||||
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
|
Point3 landmark4(5, -0.5, 1);
|
||||||
// sharedKs.push_back(sharedK);
|
|
||||||
// sharedKs.push_back(sharedK);
|
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
|
||||||
// sharedKs.push_back(sharedK);
|
measurements_cam4;
|
||||||
//
|
|
||||||
// // add fourth landmark
|
// Project 4 landmarks into three cameras
|
||||||
// Point3 landmark4(5, -0.5, 1);
|
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||||
//
|
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||||
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3,
|
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||||
// measurements_cam4;
|
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
|
||||||
//
|
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
|
||||||
// // Project 4 landmarks into three cameras
|
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
SmartProjectionParams params;
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
params.setLinearizationMode(gtsam::HESSIAN);
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
|
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||||
// measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
|
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||||
//
|
|
||||||
// SmartProjectionParams params;
|
Cameras cameraRig; // single camera in the rig
|
||||||
// params.setLinearizationMode(gtsam::HESSIAN);
|
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||||
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
FastVector<size_t> cameraIds { 0, 0, 0};
|
||||||
// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
|
||||||
// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params));
|
||||||
//
|
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||||
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params));
|
|
||||||
// smartFactor1->add(measurements_cam1, views, sharedKs);
|
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params));
|
||||||
//
|
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||||
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params));
|
|
||||||
// smartFactor2->add(measurements_cam2, views, sharedKs);
|
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params));
|
||||||
//
|
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||||
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params));
|
|
||||||
// smartFactor3->add(measurements_cam3, views, sharedKs);
|
SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params));
|
||||||
//
|
smartFactor4->add(measurements_cam4, views, cameraIds);
|
||||||
// SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, params));
|
|
||||||
// smartFactor4->add(measurements_cam4, views, sharedKs);
|
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||||
//
|
|
||||||
// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
NonlinearFactorGraph graph;
|
||||||
//
|
graph.push_back(smartFactor1);
|
||||||
// NonlinearFactorGraph graph;
|
graph.push_back(smartFactor2);
|
||||||
// graph.push_back(smartFactor1);
|
graph.push_back(smartFactor3);
|
||||||
// graph.push_back(smartFactor2);
|
graph.push_back(smartFactor4);
|
||||||
// graph.push_back(smartFactor3);
|
graph.addPrior(x1, cam1.pose(), noisePrior);
|
||||||
// graph.push_back(smartFactor4);
|
graph.addPrior(x2, cam2.pose(), noisePrior);
|
||||||
// graph.addPrior(x1, cam1.pose(), noisePrior);
|
|
||||||
// graph.addPrior(x2, cam2.pose(), noisePrior);
|
Values values;
|
||||||
//
|
values.insert(x1, cam1.pose());
|
||||||
// Values values;
|
values.insert(x2, cam2.pose());
|
||||||
// values.insert(x1, cam1.pose());
|
values.insert(x3, cam3.pose());
|
||||||
// values.insert(x2, cam2.pose());
|
|
||||||
// values.insert(x3, cam3.pose());
|
// All factors are disabled and pose should remain where it is
|
||||||
//
|
Values result;
|
||||||
// // All factors are disabled and pose should remain where it is
|
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||||
// Values result;
|
result = optimizer.optimize();
|
||||||
// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
|
||||||
// result = optimizer.optimize();
|
}
|
||||||
// EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
|
|
||||||
//}
|
/* *************************************************************************/
|
||||||
//
|
TEST( SmartProjectionRigFactor, CheckHessian) {
|
||||||
///* *************************************************************************/
|
|
||||||
//TEST( SmartProjectionRigFactor, CheckHessian) {
|
KeyVector views { x1, x2, x3 };
|
||||||
//
|
|
||||||
// KeyVector views { x1, x2, x3 };
|
using namespace vanillaPose;
|
||||||
//
|
|
||||||
// using namespace vanillaPose;
|
// Two slightly different cameras
|
||||||
//
|
Pose3 pose2 = level_pose
|
||||||
// // Two slightly different cameras
|
* Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
|
||||||
// Pose3 pose2 = level_pose
|
Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
|
||||||
// * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
|
Camera cam2(pose2, sharedK);
|
||||||
// Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0));
|
Camera cam3(pose3, sharedK);
|
||||||
// Camera cam2(pose2, sharedK);
|
|
||||||
// Camera cam3(pose3, sharedK);
|
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||||
//
|
|
||||||
// Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
// Project three landmarks into three cameras
|
||||||
//
|
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||||
// // Project three landmarks into three cameras
|
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
|
||||||
// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
SmartProjectionParams params;
|
||||||
//
|
params.setRankTolerance(10);
|
||||||
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs;
|
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||||
// sharedKs.push_back(sharedK);
|
|
||||||
// sharedKs.push_back(sharedK);
|
Cameras cameraRig; // single camera in the rig
|
||||||
// sharedKs.push_back(sharedK);
|
cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
|
||||||
//
|
FastVector<size_t> cameraIds { 0, 0, 0};
|
||||||
// SmartProjectionParams params;
|
|
||||||
// params.setRankTolerance(10);
|
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
|
||||||
// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||||
//
|
|
||||||
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, params)); // HESSIAN, by default
|
SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
|
||||||
// smartFactor1->add(measurements_cam1, views, sharedKs);
|
smartFactor2->add(measurements_cam2, views, cameraIds);
|
||||||
//
|
|
||||||
// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, params)); // HESSIAN, by default
|
SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default
|
||||||
// smartFactor2->add(measurements_cam2, views, sharedKs);
|
smartFactor3->add(measurements_cam3, views, cameraIds);
|
||||||
//
|
|
||||||
// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, params)); // HESSIAN, by default
|
NonlinearFactorGraph graph;
|
||||||
// smartFactor3->add(measurements_cam3, views, sharedKs);
|
graph.push_back(smartFactor1);
|
||||||
//
|
graph.push_back(smartFactor2);
|
||||||
// NonlinearFactorGraph graph;
|
graph.push_back(smartFactor3);
|
||||||
// graph.push_back(smartFactor1);
|
|
||||||
// graph.push_back(smartFactor2);
|
// 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
|
||||||
// graph.push_back(smartFactor3);
|
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
||||||
//
|
Point3(0.1, 0.1, 0.1)); // smaller noise
|
||||||
// // 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
|
Values values;
|
||||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
|
values.insert(x1, cam1.pose());
|
||||||
// Point3(0.1, 0.1, 0.1)); // smaller noise
|
values.insert(x2, cam2.pose());
|
||||||
// Values values;
|
// initialize third pose with some noise, we expect it to move back to original pose_above
|
||||||
// values.insert(x1, cam1.pose());
|
values.insert(x3, pose3 * noise_pose);
|
||||||
// values.insert(x2, cam2.pose());
|
EXPECT(
|
||||||
// // initialize third pose with some noise, we expect it to move back to original pose_above
|
assert_equal(
|
||||||
// values.insert(x3, pose3 * noise_pose);
|
Pose3(
|
||||||
// EXPECT(
|
Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
|
||||||
// assert_equal(
|
-0.130426831, -0.0115837907, 0.130819108, -0.98278564,
|
||||||
// Pose3(
|
-0.130455917),
|
||||||
// Rot3(0.00563056869, -0.130848107, 0.991386438, -0.991390265,
|
Point3(0.0897734171, -0.110201006, 0.901022872)),
|
||||||
// -0.130426831, -0.0115837907, 0.130819108, -0.98278564,
|
values.at<Pose3>(x3)));
|
||||||
// -0.130455917),
|
|
||||||
// Point3(0.0897734171, -0.110201006, 0.901022872)),
|
boost::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
|
||||||
// values.at<Pose3>(x3)));
|
boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
|
||||||
//
|
boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);
|
||||||
// boost::shared_ptr<GaussianFactor> factor1 = smartFactor1->linearize(values);
|
|
||||||
// boost::shared_ptr<GaussianFactor> factor2 = smartFactor2->linearize(values);
|
Matrix CumulativeInformation = factor1->information() + factor2->information()
|
||||||
// boost::shared_ptr<GaussianFactor> factor3 = smartFactor3->linearize(values);
|
+ factor3->information();
|
||||||
//
|
|
||||||
// Matrix CumulativeInformation = factor1->information() + factor2->information()
|
boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
|
||||||
// + factor3->information();
|
values);
|
||||||
//
|
Matrix GraphInformation = GaussianGraph->hessian().first;
|
||||||
// boost::shared_ptr<GaussianFactorGraph> GaussianGraph = graph.linearize(
|
|
||||||
// values);
|
// Check Hessian
|
||||||
// Matrix GraphInformation = GaussianGraph->hessian().first;
|
EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
|
||||||
//
|
|
||||||
// // Check Hessian
|
Matrix AugInformationMatrix = factor1->augmentedInformation()
|
||||||
// EXPECT(assert_equal(GraphInformation, CumulativeInformation, 1e-6));
|
+ factor2->augmentedInformation() + factor3->augmentedInformation();
|
||||||
//
|
|
||||||
// Matrix AugInformationMatrix = factor1->augmentedInformation()
|
// Check Information vector
|
||||||
// + factor2->augmentedInformation() + factor3->augmentedInformation();
|
Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
|
||||||
//
|
|
||||||
// // Check Information vector
|
// Check Hessian
|
||||||
// Vector InfoVector = AugInformationMatrix.block(0, 18, 18, 1); // 18x18 Hessian + information vector
|
EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
|
||||||
//
|
}
|
||||||
// // Check Hessian
|
|
||||||
// EXPECT(assert_equal(InfoVector, GaussianGraph->hessian().second, 1e-6));
|
/* *************************************************************************/
|
||||||
//}
|
TEST( SmartProjectionRigFactor, Hessian ) {
|
||||||
//
|
|
||||||
///* *************************************************************************/
|
using namespace vanillaPose2;
|
||||||
//TEST( SmartProjectionRigFactor, Hessian ) {
|
|
||||||
//
|
KeyVector views { x1, x2 };
|
||||||
// using namespace vanillaPose2;
|
|
||||||
//
|
// Project three landmarks into 2 cameras
|
||||||
// KeyVector views { x1, x2 };
|
Point2 cam1_uv1 = cam1.project(landmark1);
|
||||||
//
|
Point2 cam2_uv1 = cam2.project(landmark1);
|
||||||
// // Project three landmarks into 2 cameras
|
Point2Vector measurements_cam1;
|
||||||
// Point2 cam1_uv1 = cam1.project(landmark1);
|
measurements_cam1.push_back(cam1_uv1);
|
||||||
// Point2 cam2_uv1 = cam2.project(landmark1);
|
measurements_cam1.push_back(cam2_uv1);
|
||||||
// Point2Vector measurements_cam1;
|
|
||||||
// measurements_cam1.push_back(cam1_uv1);
|
Cameras cameraRig; // single camera in the rig
|
||||||
// measurements_cam1.push_back(cam2_uv1);
|
cameraRig.push_back( Camera(Pose3::identity(), sharedK2) );
|
||||||
//
|
FastVector<size_t> cameraIds { 0, 0 };
|
||||||
// std::vector < boost::shared_ptr < Cal3_S2 >> sharedK2s;
|
|
||||||
// sharedK2s.push_back(sharedK2);
|
SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig));
|
||||||
// sharedK2s.push_back(sharedK2);
|
smartFactor1->add(measurements_cam1, views, cameraIds);
|
||||||
//
|
|
||||||
// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model));
|
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
|
||||||
// smartFactor1->add(measurements_cam1, views, sharedK2s);
|
Point3(0.5, 0.1, 0.3));
|
||||||
//
|
Values values;
|
||||||
// Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10),
|
values.insert(x1, cam1.pose());
|
||||||
// Point3(0.5, 0.1, 0.3));
|
values.insert(x2, cam2.pose());
|
||||||
// Values values;
|
|
||||||
// values.insert(x1, cam1.pose());
|
boost::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values);
|
||||||
// values.insert(x2, cam2.pose());
|
|
||||||
//
|
// compute triangulation from linearization point
|
||||||
// boost::shared_ptr<GaussianFactor> factor = smartFactor1->linearize(values);
|
// compute reprojection errors (sum squared)
|
||||||
//
|
// compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
|
||||||
// // compute triangulation from linearization point
|
// check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
|
||||||
// // compute reprojection errors (sum squared)
|
}
|
||||||
// // compare with factor.info(): the bottom right element is the squared sum of the reprojection errors (normalized by the covariance)
|
|
||||||
// // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4]
|
|
||||||
//}
|
|
||||||
//
|
|
||||||
///* ************************************************************************* */
|
///* ************************************************************************* */
|
||||||
//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
|
//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
|
||||||
// using namespace bundlerPose;
|
// using namespace bundlerPose;
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue