From 3758fdaa5d7e92d5b45f3b9defe075afce6d46d9 Mon Sep 17 00:00:00 2001 From: lcarlone Date: Sat, 2 Oct 2021 23:10:05 -0400 Subject: [PATCH] all tests work except serialization --- gtsam/slam/SmartProjectionRigFactor.h | 25 +- gtsam/slam/tests/smartFactorScenarios.h | 8 +- .../tests/testSmartProjectionRigFactor.cpp | 754 +++++++++--------- .../SmartProjectionPoseFactorRollingShutter.h | 6 +- 4 files changed, 395 insertions(+), 398 deletions(-) diff --git a/gtsam/slam/SmartProjectionRigFactor.h b/gtsam/slam/SmartProjectionRigFactor.h index 2ec2ed86f..581859b1f 100644 --- a/gtsam/slam/SmartProjectionRigFactor.h +++ b/gtsam/slam/SmartProjectionRigFactor.h @@ -38,7 +38,7 @@ namespace gtsam { /** * This factor assumes that camera calibration is fixed (but each camera * measurement can have a different extrinsic and intrinsic calibration). - * The factor only constrains poses (variable dimension is 6). + * The factor only constrains poses (variable dimension is 6 for each pose). * This factor requires that values contains the involved poses (Pose3). * If all measurements share the same calibration (i.e., are from the same camera), use SmartProjectionPoseFactor instead! * If the calibration should be optimized, as well, use SmartProjectionFactor instead! @@ -60,7 +60,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { /// vector of keys (one for each observation) with potentially repeated keys KeyVector nonUniqueKeys_; - /// cameras in the rig (fixed poses wrt body + fixed intrinsics) + /// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each camera) typename Base::Cameras cameraRig_; /// vector of camera Ids (one for each observation), identifying which camera took the measurement @@ -185,13 +185,12 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { */ typename Base::Cameras cameras(const Values& values) const override { typename Base::Cameras cameras; + cameras.reserve(nonUniqueKeys_.size()); // preallocate for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { - const Key cameraId = cameraIds_[i]; - const Pose3& body_P_cam_i = cameraRig_[cameraId].pose(); const Pose3 world_P_sensor_i = values.at(nonUniqueKeys_[i]) - * body_P_cam_i; + * cameraRig_[ cameraIds_[i] ].pose(); // cameraRig_[ cameraIds_[i] ].pose() is body_P_cam_i cameras.emplace_back(world_P_sensor_i, - make_shared(cameraRig_[cameraId].calibration())); + make_shared(cameraRig_[ cameraIds_[i] ].calibration())); } return cameras; } @@ -223,10 +222,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { } else { // valid result: compute jacobians b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); for (size_t i = 0; i < Fs.size(); i++) { - const Key cameraId = cameraIds_[i]; - const Pose3 body_P_sensor = cameraRig_[cameraId].pose(); - const Pose3 sensor_P_body = body_P_sensor.inverse(); - const Pose3 world_P_body = cameras[i].pose() * sensor_P_body; + const Pose3& body_P_sensor = cameraRig_[ cameraIds_[i] ].pose(); + const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); Eigen::Matrix H; world_P_body.compose(body_P_sensor, H); Fs.at(i) = Fs.at(i) * H; @@ -246,11 +243,11 @@ class SmartProjectionRigFactor : public SmartProjectionFactor { Cameras cameras = this->cameras(values); // Create structures for Hessian Factors - FastVector js; - FastVector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); - FastVector < Vector > gs(nrUniqueKeys); + std::vector js; + std::vector < Matrix > Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); + std::vector < Vector > gs(nrUniqueKeys); - if (this->measured_.size() != this->cameras(values).size()) // 1 observation per camera + if (this->measured_.size() != cameras.size()) // 1 observation per camera throw std::runtime_error("SmartProjectionRigFactor: " "measured_.size() inconsistent with input"); diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index a9d0c43f7..b17ffdac6 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -71,7 +71,7 @@ namespace vanillaPose { typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Camera level_camera(level_pose, sharedK); Camera level_camera_right(pose_right, sharedK); @@ -86,7 +86,7 @@ namespace vanillaPose2 { typedef PinholePose Camera; typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static Cal3_S2::shared_ptr sharedK2(new Cal3_S2(1500, 1200, 0, 640, 480)); Camera level_camera(level_pose, sharedK2); Camera level_camera_right(pose_right, sharedK2); @@ -99,6 +99,7 @@ Camera cam3(pose_above, sharedK2); // Cal3Bundler cameras namespace bundler { typedef PinholeCamera Camera; +typedef CameraSet Cameras; typedef SmartProjectionFactor SmartFactor; static Cal3Bundler K(500, 1e-3, 1e-3, 0, 0); Camera level_camera(level_pose, K); @@ -115,8 +116,9 @@ typedef GeneralSFMFactor SFMFactor; // Cal3Bundler poses namespace bundlerPose { typedef PinholePose Camera; +typedef CameraSet Cameras; typedef SmartProjectionPoseFactor SmartFactor; -typedef SmartProjectionRigFactor SmartFactorP; +typedef SmartProjectionRigFactor SmartRigFactor; static boost::shared_ptr sharedBundlerK( new Cal3Bundler(500, 1e-3, 1e-3, 1000, 2000)); Camera level_camera(level_pose, sharedBundlerK); diff --git a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp index 4c6b33655..db077525b 100644 --- a/gtsam/slam/tests/testSmartProjectionRigFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionRigFactor.cpp @@ -60,7 +60,7 @@ TEST( SmartProjectionRigFactor, Constructor) { using namespace vanillaPose; Cameras cameraRig; cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); } /* ************************************************************************* */ @@ -69,7 +69,7 @@ TEST( SmartProjectionRigFactor, Constructor2) { Cameras cameraRig; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorP factor1(model, cameraRig, params); + SmartRigFactor factor1(model, cameraRig, params); } /* ************************************************************************* */ @@ -77,7 +77,7 @@ TEST( SmartProjectionRigFactor, Constructor3) { using namespace vanillaPose; Cameras cameraRig; cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); } @@ -88,7 +88,7 @@ TEST( SmartProjectionRigFactor, Constructor4) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorP factor1(model, cameraRig, params); + SmartRigFactor factor1(model, cameraRig, params); factor1.add(measurement1, x1, cameraId1); } @@ -98,10 +98,10 @@ TEST( SmartProjectionRigFactor, Equals ) { Cameras cameraRig; // single camera in the rig cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); - SmartFactorP::shared_ptr factor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor1(new SmartRigFactor(model, cameraRig)); factor1->add(measurement1, x1, cameraId1); - SmartFactorP::shared_ptr factor2(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model, cameraRig)); factor2->add(measurement1, x1, cameraId1); CHECK(assert_equal(*factor1, *factor2)); @@ -119,7 +119,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { Point2 level_uv = level_camera.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1); - SmartFactorP factor(model, cameraRig); + SmartRigFactor factor(model, cameraRig); factor.add(level_uv, x1, cameraId1); // both taken from the same camera factor.add(level_uv_right, x2, cameraId1); @@ -131,13 +131,13 @@ TEST( SmartProjectionRigFactor, noiseless ) { double expectedError = 0.0; EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - SmartFactorP::Cameras cameras = factor.cameras(values); + SmartRigFactor::Cameras cameras = factor.cameras(values); double actualError2 = factor.totalReprojectionError(cameras); EXPECT_DOUBLES_EQUAL(expectedError, actualError2, 1e-7); // Calculate expected derivative for point (easiest to check) std::function f = // - std::bind(&SmartFactorP::whitenedError, factor, cameras, + std::bind(&SmartRigFactor::whitenedError, factor, cameras, std::placeholders::_1); // Calculate using computeEP @@ -153,7 +153,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { EXPECT(assert_equal(expectedE, actualE, 1e-7)); // Calculate using reprojectionError - SmartFactorP::Cameras::FBlocks F; + SmartRigFactor::Cameras::FBlocks F; Matrix E; Vector actualErrors = factor.unwhitenedError(cameras, *point, F, E); EXPECT(assert_equal(expectedE, E, 1e-7)); @@ -162,7 +162,7 @@ TEST( SmartProjectionRigFactor, noiseless ) { // Calculate using computeJacobians Vector b; - SmartFactorP::FBlocks Fs; + SmartRigFactor::FBlocks Fs; factor.computeJacobians(Fs, E, b, cameras, *point); double actualError3 = b.squaredNorm(); EXPECT(assert_equal(expectedE, E, 1e-7)); @@ -188,14 +188,14 @@ TEST( SmartProjectionRigFactor, noisy ) { Point3(0.5, 0.1, 0.3)); values.insert(x2, pose_right.compose(noise_pose)); - SmartFactorP::shared_ptr factor(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr factor(new SmartRigFactor(model,cameraRig)); factor->add(level_uv, x1, cameraId1); factor->add(level_uv_right, x2, cameraId1); double actualError1 = factor->error(values); // create other factor by passing multiple measurements - SmartFactorP::shared_ptr factor2(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr factor2(new SmartRigFactor(model,cameraRig)); Point2Vector measurements; measurements.push_back(level_uv); @@ -244,13 +244,13 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) { params.setDegeneracyMode(IGNORE_DEGENERACY); params.setEnableEPI(false); - SmartFactorP smartFactor1(model, cameraRig, params); + SmartRigFactor smartFactor1(model, cameraRig, params); smartFactor1.add(measurements_cam1, views, cameraIds); - SmartFactorP smartFactor2(model, cameraRig, params); + SmartRigFactor smartFactor2(model, cameraRig, params); smartFactor2.add(measurements_cam2, views, cameraIds); - SmartFactorP smartFactor3(model, cameraRig, params); + SmartRigFactor smartFactor3(model, cameraRig, params); smartFactor3.add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -305,13 +305,13 @@ TEST( SmartProjectionRigFactor, 3poses_smart_projection_factor ) { KeyVector views {x1,x2,x3}; FastVector cameraIds{0,0,0};// 3 measurements from the same camera in the rig - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model,cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model,cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model,cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model,cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -378,11 +378,11 @@ TEST( SmartProjectionRigFactor, Factors ) { KeyVector views { x1, x2 }; FastVector cameraIds { 0, 0 }; - SmartFactorP::shared_ptr smartFactor1 = boost::make_shared < SmartFactorP + SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared < SmartRigFactor > (model,cameraRig); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::Cameras cameras; + SmartRigFactor::Cameras cameras; cameras.push_back(cam1); cameras.push_back(cam2); @@ -465,13 +465,13 @@ TEST( SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor ) { Cameras cameraRig; // single camera in the rig cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -533,13 +533,13 @@ TEST( SmartProjectionRigFactor, landmarkDistance ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -599,16 +599,16 @@ TEST( SmartProjectionRigFactor, dynamicOutlierRejection ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); smartFactor3->add(measurements_cam3, views, cameraIds); - SmartFactorP::shared_ptr smartFactor4(new SmartFactorP(model, cameraRig, params)); + SmartRigFactor::shared_ptr smartFactor4(new SmartRigFactor(model, cameraRig, params)); smartFactor4->add(measurements_cam4, views, cameraIds); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -662,13 +662,13 @@ TEST( SmartProjectionRigFactor, CheckHessian) { cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); FastVector cameraIds { 0, 0, 0}; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, cameraIds); - SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, cameraIds); - SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model, cameraRig, params)); // HESSIAN, by default + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, cameraIds); NonlinearFactorGraph graph; @@ -735,7 +735,7 @@ TEST( SmartProjectionRigFactor, Hessian ) { cameraRig.push_back( Camera(Pose3::identity(), sharedK2) ); FastVector cameraIds { 0, 0 }; - SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model, cameraRig)); + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); smartFactor1->add(measurements_cam1, views, cameraIds); Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 10, 0., -M_PI / 10), @@ -752,347 +752,340 @@ TEST( SmartProjectionRigFactor, Hessian ) { // check that it is correctly scaled when using noiseProjection = [1/4 0; 0 1/4] } -///* ************************************************************************* */ -//TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { -// using namespace bundlerPose; -// SmartProjectionParams params; -// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); -// SmartFactorP factor(model, params); -// factor.add(measurement1, x1, sharedBundlerK); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, Cal3Bundler ) { -// -// using namespace bundlerPose; -// -// // three landmarks ~5 meters in front of camera -// Point3 landmark3(3, 0, 3.0); -// -// 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); -// -// KeyVector views { x1, x2, x3 }; -// -// std::vector < boost::shared_ptr < Cal3Bundler >> sharedBundlerKs; -// sharedBundlerKs.push_back(sharedBundlerK); -// sharedBundlerKs.push_back(sharedBundlerK); -// sharedBundlerKs.push_back(sharedBundlerK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_cam1, views, sharedBundlerKs); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_cam2, views, sharedBundlerKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_cam3, views, sharedBundlerKs); -// -// 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()); -// // 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(0, -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(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); -//} -// -//#include -//typedef GenericProjectionFactor TestProjectionFactor; -//static Symbol l0('L', 0); -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { -// // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark -// // at a single pose, a setup that occurs in multi-camera systems -// -// using namespace vanillaPose; -// Point2Vector measurements_lmk1; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// -// // create redundant measurements: -// Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; -// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement -// -// // create inputs -// std::vector keys; -// keys.push_back(x1); -// keys.push_back(x2); -// keys.push_back(x3); -// keys.push_back(x1); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_lmk1_redundant, keys, sharedKs); -// -// 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 to get a nontrivial linearization point -// values.insert(x3, pose_above * noise_pose); -// EXPECT( // check that the pose is actually noisy -// assert_equal( Pose3( Rot3(0, -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(x3))); -// -// // linearization point for the poses -// Pose3 pose1 = level_pose; -// Pose3 pose2 = pose_right; -// Pose3 pose3 = pose_above * noise_pose; -// -// // ==== check Hessian of smartFactor1 ===== -// // -- compute actual Hessian -// boost::shared_ptr linearfactor1 = smartFactor1->linearize( -// values); -// Matrix actualHessian = linearfactor1->information(); -// -// // -- compute expected Hessian from manual Schur complement from Jacobians -// // linearization point for the 3D point -// smartFactor1->triangulateSafe(smartFactor1->cameras(values)); -// TriangulationResult point = smartFactor1->point(); -// EXPECT(point.valid()); // check triangulated point is valid -// -// // Use standard ProjectionFactor factor to calculate the Jacobians -// Matrix F = Matrix::Zero(2 * 4, 6 * 3); -// Matrix E = Matrix::Zero(2 * 4, 3); -// Vector b = Vector::Zero(2 * 4); -// -// // create projection factors rolling shutter -// TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, -// sharedK); -// Matrix HPoseActual, HEActual; -// // note: b is minus the reprojection error, cf the smart factor jacobian computation -// b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(0, 0) = HPoseActual; -// E.block<2, 3>(0, 0) = HEActual; -// -// TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, -// sharedK); -// b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(2, 6) = HPoseActual; -// E.block<2, 3>(2, 0) = HEActual; -// -// TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, -// sharedK); -// b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(4, 12) = HPoseActual; -// E.block<2, 3>(4, 0) = HEActual; -// -// TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, -// sharedK); -// b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, -// HEActual); -// F.block<2, 6>(6, 0) = HPoseActual; -// E.block<2, 3>(6, 0) = HEActual; -// -// // whiten -// F = (1 / sigma) * F; -// E = (1 / sigma) * E; -// b = (1 / sigma) * b; -// //* G = F' * F - F' * E * P * E' * F -// Matrix P = (E.transpose() * E).inverse(); -// Matrix expectedHessian = F.transpose() * F -// - (F.transpose() * E * P * E.transpose() * F); -// EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); -// -// // ==== check Information vector of smartFactor1 ===== -// GaussianFactorGraph gfg; -// gfg.add(linearfactor1); -// Matrix actualHessian_v2 = gfg.hessian().first; -// EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian -// -// // -- compute actual information vector -// Vector actualInfoVector = gfg.hessian().second; -// -// // -- compute expected information vector from manual Schur complement from Jacobians -// //* g = F' * (b - E * P * E' * b) -// Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); -// EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); -// -// // ==== check error of smartFactor1 (again) ===== -// NonlinearFactorGraph nfg_projFactors; -// nfg_projFactors.add(factor11); -// nfg_projFactors.add(factor12); -// nfg_projFactors.add(factor13); -// nfg_projFactors.add(factor14); -// values.insert(l0, *point); -// -// double actualError = smartFactor1->error(values); -// double expectedError = nfg_projFactors.error(values); -// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -//} -// -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { -// -// using namespace vanillaPose; -// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; -// -// // Project three landmarks into three cameras -// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); -// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); -// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); -// -// // create inputs -// std::vector keys; -// keys.push_back(x1); -// keys.push_back(x2); -// keys.push_back(x3); -// -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs; -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// sharedKs.push_back(sharedK); -// -// // For first factor, we create redundant measurement (taken by the same keys as factor 1, to -// // make sure the redundancy in the keys does not create problems) -// Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; -// measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement -// std::vector keys_redundant = keys; -// keys_redundant.push_back(keys.at(0)); // we readd the first key -// std::vector < boost::shared_ptr < Cal3_S2 >> sharedKs_redundant = sharedKs; -// sharedKs_redundant.push_back(sharedK);// we readd the first calibration -// -// SmartFactorP::shared_ptr smartFactor1(new SmartFactorP(model)); -// smartFactor1->add(measurements_lmk1_redundant, keys_redundant, sharedKs_redundant); -// -// SmartFactorP::shared_ptr smartFactor2(new SmartFactorP(model)); -// smartFactor2->add(measurements_lmk2, keys, sharedKs); -// -// SmartFactorP::shared_ptr smartFactor3(new SmartFactorP(model)); -// smartFactor3->add(measurements_lmk3, keys, sharedKs); -// -// 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, level_pose, noisePrior); -// graph.addPrior(x2, pose_right, noisePrior); -// -// Values groundTruth; -// groundTruth.insert(x1, level_pose); -// groundTruth.insert(x2, pose_right); -// groundTruth.insert(x3, pose_above); -// DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); -// -// // 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, 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); -// EXPECT( // check that the pose is actually noisy -// assert_equal( -// Pose3( -// Rot3(0, -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(x3))); -// -// Values result; -// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); -// result = optimizer.optimize(); -// EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); -//} -// -//#ifndef DISABLE_TIMING -//#include -//// this factor is actually slightly faster (but comparable) to original SmartProjectionPoseFactor -////-Total: 0 CPU (0 times, 0 wall, 0.01 children, min: 0 max: 0) -////| -SmartFactorP LINEARIZE: 0 CPU (1000 times, 0.005481 wall, 0 children, min: 0 max: 0) -////| -SmartPoseFactor LINEARIZE: 0.01 CPU (1000 times, 0.007318 wall, 0.01 children, min: 0 max: 0) -///* *************************************************************************/ -//TEST( SmartProjectionRigFactor, timing ) { -// -// using namespace vanillaPose; -// -// // Default cameras for simple derivatives -// static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); -// -// Rot3 R = Rot3::identity(); -// Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); -// Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); -// Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); -// Pose3 body_P_sensorId = Pose3::identity(); -// -// // one landmarks 1m in front of camera -// Point3 landmark1(0, 0, 10); -// -// Point2Vector measurements_lmk1; -// -// // Project 2 landmarks into 2 cameras -// measurements_lmk1.push_back(cam1.project(landmark1)); -// measurements_lmk1.push_back(cam2.project(landmark1)); -// -// size_t nrTests = 1000; -// -// for(size_t i = 0; iadd(measurements_lmk1[0], x1, sharedKSimple, body_P_sensorId); -// smartFactorP->add(measurements_lmk1[1], x1, sharedKSimple, body_P_sensorId); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(SmartFactorP_LINEARIZE); -// smartFactorP->linearize(values); -// gttoc_(SmartFactorP_LINEARIZE); -// } -// -// for(size_t i = 0; iadd(measurements_lmk1[0], x1); -// smartFactor->add(measurements_lmk1[1], x2); -// -// Values values; -// values.insert(x1, pose1); -// values.insert(x2, pose2); -// gttic_(SmartPoseFactor_LINEARIZE); -// smartFactor->linearize(values); -// gttoc_(SmartPoseFactor_LINEARIZE); -// } -// tictoc_print_(); -//} -//#endif -// +/* ************************************************************************* */ +TEST( SmartProjectionRigFactor, ConstructorWithCal3Bundler) { + using namespace bundlerPose; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartRigFactor factor(model, cameraRig, params); + factor.add(measurement1, x1, cameraId1); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, Cal3Bundler ) { + + using namespace bundlerPose; + + // three landmarks ~5 meters in front of camera + Point3 landmark3(3, 0, 3.0); + + 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); + + KeyVector views { x1, x2, x3 }; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedBundlerK) ); + FastVector cameraIds { 0, 0, 0 }; + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_cam1, views, cameraIds); + + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + smartFactor2->add(measurements_cam2, views, cameraIds); + + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + smartFactor3->add(measurements_cam3, views, cameraIds); + + 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()); + // 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(0, -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(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); +} + +#include +typedef GenericProjectionFactor TestProjectionFactor; +static Symbol l0('L', 0); +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, hessianComparedToProjFactors_measurementsFromSamePose) { + // in this test we make sure the fact works even if we have multiple pixel measurements of the same landmark + // at a single pose, a setup that occurs in multi-camera systems + + using namespace vanillaPose; + Point2Vector measurements_lmk1; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + + // create redundant measurements: + Camera::MeasurementVector measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + + // create inputs + std::vector keys { x1, x2, x3, x1}; + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0, 0 }; + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_lmk1_redundant, keys, cameraIds); + + 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 to get a nontrivial linearization point + values.insert(x3, pose_above * noise_pose); + EXPECT( // check that the pose is actually noisy + assert_equal( Pose3( Rot3(0, -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(x3))); + + // linearization point for the poses + Pose3 pose1 = level_pose; + Pose3 pose2 = pose_right; + Pose3 pose3 = pose_above * noise_pose; + + // ==== check Hessian of smartFactor1 ===== + // -- compute actual Hessian + boost::shared_ptr linearfactor1 = smartFactor1->linearize( + values); + Matrix actualHessian = linearfactor1->information(); + + // -- compute expected Hessian from manual Schur complement from Jacobians + // linearization point for the 3D point + smartFactor1->triangulateSafe(smartFactor1->cameras(values)); + TriangulationResult point = smartFactor1->point(); + EXPECT(point.valid()); // check triangulated point is valid + + // Use standard ProjectionFactor factor to calculate the Jacobians + Matrix F = Matrix::Zero(2 * 4, 6 * 3); + Matrix E = Matrix::Zero(2 * 4, 3); + Vector b = Vector::Zero(2 * 4); + + // create projection factors rolling shutter + TestProjectionFactor factor11(measurements_lmk1_redundant[0], model, x1, l0, + sharedK); + Matrix HPoseActual, HEActual; + // note: b is minus the reprojection error, cf the smart factor jacobian computation + b.segment<2>(0) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(0, 0) = HPoseActual; + E.block<2, 3>(0, 0) = HEActual; + + TestProjectionFactor factor12(measurements_lmk1_redundant[1], model, x2, l0, + sharedK); + b.segment<2>(2) = -factor12.evaluateError(pose2, *point, HPoseActual, + HEActual); + F.block<2, 6>(2, 6) = HPoseActual; + E.block<2, 3>(2, 0) = HEActual; + + TestProjectionFactor factor13(measurements_lmk1_redundant[2], model, x3, l0, + sharedK); + b.segment<2>(4) = -factor13.evaluateError(pose3, *point, HPoseActual, + HEActual); + F.block<2, 6>(4, 12) = HPoseActual; + E.block<2, 3>(4, 0) = HEActual; + + TestProjectionFactor factor14(measurements_lmk1_redundant[3], model, x1, l0, + sharedK); + b.segment<2>(6) = -factor11.evaluateError(pose1, *point, HPoseActual, + HEActual); + F.block<2, 6>(6, 0) = HPoseActual; + E.block<2, 3>(6, 0) = HEActual; + + // whiten + F = (1 / sigma) * F; + E = (1 / sigma) * E; + b = (1 / sigma) * b; + //* G = F' * F - F' * E * P * E' * F + Matrix P = (E.transpose() * E).inverse(); + Matrix expectedHessian = F.transpose() * F + - (F.transpose() * E * P * E.transpose() * F); + EXPECT(assert_equal(expectedHessian, actualHessian, 1e-6)); + + // ==== check Information vector of smartFactor1 ===== + GaussianFactorGraph gfg; + gfg.add(linearfactor1); + Matrix actualHessian_v2 = gfg.hessian().first; + EXPECT(assert_equal(actualHessian_v2, actualHessian, 1e-6)); // sanity check on hessian + + // -- compute actual information vector + Vector actualInfoVector = gfg.hessian().second; + + // -- compute expected information vector from manual Schur complement from Jacobians + //* g = F' * (b - E * P * E' * b) + Vector expectedInfoVector = F.transpose() * (b - E * P * E.transpose() * b); + EXPECT(assert_equal(expectedInfoVector, actualInfoVector, 1e-6)); + + // ==== check error of smartFactor1 (again) ===== + NonlinearFactorGraph nfg_projFactors; + nfg_projFactors.add(factor11); + nfg_projFactors.add(factor12); + nfg_projFactors.add(factor13); + nfg_projFactors.add(factor14); + values.insert(l0, *point); + + double actualError = smartFactor1->error(values); + double expectedError = nfg_projFactors.error(values); + EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +} + +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose ) { + + using namespace vanillaPose; + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // Project three landmarks into three cameras + projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); + projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); + projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); + + // create inputs + std::vector keys { x1, x2, x3 }; + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); + FastVector cameraIds { 0, 0, 0 }; + FastVector cameraIdsRedundant { 0, 0, 0, 0 }; + + // For first factor, we create redundant measurement (taken by the same keys as factor 1, to + // make sure the redundancy in the keys does not create problems) + Camera::MeasurementVector& measurements_lmk1_redundant = measurements_lmk1; + measurements_lmk1_redundant.push_back(measurements_lmk1.at(0)); // we readd the first measurement + std::vector keys_redundant = keys; + keys_redundant.push_back(keys.at(0)); // we readd the first key + + SmartRigFactor::shared_ptr smartFactor1(new SmartRigFactor(model, cameraRig)); + smartFactor1->add(measurements_lmk1_redundant, keys_redundant, cameraIdsRedundant); + + SmartRigFactor::shared_ptr smartFactor2(new SmartRigFactor(model, cameraRig)); + smartFactor2->add(measurements_lmk2, keys, cameraIds); + + SmartRigFactor::shared_ptr smartFactor3(new SmartRigFactor(model, cameraRig)); + smartFactor3->add(measurements_lmk3, keys, cameraIds); + + 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, level_pose, noisePrior); + graph.addPrior(x2, pose_right, noisePrior); + + Values groundTruth; + groundTruth.insert(x1, level_pose); + groundTruth.insert(x2, pose_right); + groundTruth.insert(x3, pose_above); + DOUBLES_EQUAL(0, graph.error(groundTruth), 1e-9); + + // 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, 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); + EXPECT( // check that the pose is actually noisy + assert_equal( + Pose3( + Rot3(0, -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(x3))); + + Values result; + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); + result = optimizer.optimize(); + EXPECT(assert_equal(pose_above, result.at(x3), 1e-5)); +} + +#ifndef DISABLE_TIMING +#include +// this factor is slightly slower (but comparable) to original SmartProjectionPoseFactor +//-Total: 0 CPU (0 times, 0 wall, 0.17 children, min: 0 max: 0) +//| -SmartRigFactor LINEARIZE: 0.11 CPU (10000 times, 0.086311 wall, 0.11 children, min: 0 max: 0) +//| -SmartPoseFactor LINEARIZE: 0.06 CPU (10000 times, 0.065103 wall, 0.06 children, min: 0 max: 0) +/* *************************************************************************/ +TEST( SmartProjectionRigFactor, timing ) { + + using namespace vanillaPose; + + // Default cameras for simple derivatives + static Cal3_S2::shared_ptr sharedKSimple(new Cal3_S2(100, 100, 0, 0, 0)); + + Rot3 R = Rot3::identity(); + Pose3 pose1 = Pose3(R, Point3(0, 0, 0)); + Pose3 pose2 = Pose3(R, Point3(1, 0, 0)); + Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); + Pose3 body_P_sensorId = Pose3::identity(); + + Cameras cameraRig; // single camera in the rig + cameraRig.push_back( Camera(body_P_sensorId, sharedKSimple) ); + + // one landmarks 1m in front of camera + Point3 landmark1(0, 0, 10); + + Point2Vector measurements_lmk1; + + // Project 2 landmarks into 2 cameras + measurements_lmk1.push_back(cam1.project(landmark1)); + measurements_lmk1.push_back(cam2.project(landmark1)); + + size_t nrTests = 10000; + + for(size_t i = 0; iadd(measurements_lmk1[0], x1, cameraId1); + smartFactorP->add(measurements_lmk1[1], x1, cameraId1); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartRigFactor_LINEARIZE); + smartFactorP->linearize(values); + gttoc_(SmartRigFactor_LINEARIZE); + } + + for(size_t i = 0; iadd(measurements_lmk1[0], x1); + smartFactor->add(measurements_lmk1[1], x2); + + Values values; + values.insert(x1, pose1); + values.insert(x2, pose2); + gttic_(SmartPoseFactor_LINEARIZE); + smartFactor->linearize(values); + gttoc_(SmartPoseFactor_LINEARIZE); + } + tictoc_print_(); +} +#endif + ///* ************************************************************************* */ //BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained"); //BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal"); @@ -1107,7 +1100,11 @@ TEST( SmartProjectionRigFactor, Hessian ) { // using namespace gtsam::serializationTestHelpers; // SmartProjectionParams params; // params.setRankTolerance(rankTol); -// SmartFactorP factor(model, params); +// +// Cameras cameraRig; // single camera in the rig +// cameraRig.push_back( Camera(Pose3::identity(), sharedK) ); +// +// SmartRigFactor factor(model, cameraRig, params); // // EXPECT(equalsObj(factor)); // EXPECT(equalsXML(factor)); @@ -1120,14 +1117,13 @@ TEST( SmartProjectionRigFactor, Hessian ) { //// using namespace gtsam::serializationTestHelpers; //// SmartProjectionParams params; //// params.setRankTolerance(rankTol); -//// SmartFactorP factor(model, params); +//// SmartRigFactor factor(model, params); //// //// // insert some measurements //// KeyVector key_view; //// Point2Vector meas_view; //// std::vector> sharedKs; //// -//// //// key_view.push_back(Symbol('x', 1)); //// meas_view.push_back(Point2(10, 10)); //// sharedKs.push_back(sharedK); diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index 7660ff236..d58aea148 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -341,19 +341,21 @@ class SmartProjectionPoseFactorRollingShutter this->keys_ .size(); // note: by construction, keys_ only contains unique keys + typename Base::Cameras cameras = this->cameras(values); + // Create structures for Hessian Factors KeyVector js; std::vector Gs(nrUniqueKeys * (nrUniqueKeys + 1) / 2); std::vector gs(nrUniqueKeys); if (this->measured_.size() != - this->cameras(values).size()) // 1 observation per interpolated camera + cameras.size()) // 1 observation per interpolated camera throw std::runtime_error( "SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); // triangulate 3D point at given linearization point - this->triangulateSafe(this->cameras(values)); + this->triangulateSafe(cameras); if (!this->result_) { // failed: return "empty/zero" Hessian if (this->params_.degeneracyMode == ZERO_ON_DEGENERACY) {