diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index f3f8900f5..84e3437a7 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -49,9 +49,6 @@ class SmartProjectionPoseFactorRollingShutter typedef typename CAMERA::CalibrationType CALIBRATION; protected: - /// shared pointer to calibration object (one for each observation) - std::vector> K_all_; - /// The keys of the pose of the body (with respect to an external world /// frame): two consecutive poses for each observation std::vector> world_P_body_key_pairs_; @@ -60,10 +57,10 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// one or more cameras in the rig (fixed poses wrt body + fixed intrinsics) + /// one or more cameras taking observations (fixed poses wrt body + fixed intrinsics) typename Base::Cameras cameraRig_; - /// vector of camera Ids (one for each observation, in the same order), identifying which camera in the rig took the measurement + /// vector of camera Ids (one for each observation, in the same order), identifying which camera took the measurement FastVector cameraIds_; public: @@ -87,6 +84,7 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise + * @param cameraRig set of cameras (fixed poses wrt body and intrinsics) taking the measurements * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( @@ -102,6 +100,7 @@ class SmartProjectionPoseFactorRollingShutter /** * Constructor * @param Isotropic measurement noise + * @param camera single camera (fixed poses wrt body and intrinsics) * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( @@ -114,13 +113,11 @@ class SmartProjectionPoseFactorRollingShutter cameraRig_.push_back(camera); } - /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; /** - * add a new measurement, with 2 pose keys, interpolation factor, camera - * (intrinsic and extrinsic) calibration, and observed pixel. + * add a new measurement, with 2 pose keys, interpolation factor, and cameraId * @param measured 2-dimensional location of the projection of a single * landmark in a single view (the measurement), interpolated from the 2 poses * @param world_P_body_key1 key corresponding to the first body poses (time <= @@ -129,8 +126,7 @@ class SmartProjectionPoseFactorRollingShutter * >= time pixel is acquired) * @param alpha interpolation factor in [0,1], such that if alpha = 0 the * interpolated pose is the same as world_P_body_key1 - * @param K (fixed) camera intrinsic calibration - * @param body_P_sensor (fixed) camera extrinsic calibration + * @param cameraId ID of the camera taking the measurement (default 0) */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, @@ -168,8 +164,7 @@ class SmartProjectionPoseFactorRollingShutter * for the i0-th measurement can be interpolated * @param alphas vector of interpolation params (in [0,1]), one for each * measurement (in the same order) - * @param Ks vector of (fixed) intrinsic calibration objects - * @param body_P_sensors vector of (fixed) extrinsic calibration objects + * @param cameraIds IDs of the cameras taking each measurement (same order as the measurements) */ void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, @@ -223,7 +218,7 @@ class SmartProjectionPoseFactorRollingShutter const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const override { std::cout << s << "SmartProjectionPoseFactorRollingShutter: \n "; - for (size_t i = 0; i < K_all_.size(); i++) { + for (size_t i = 0; i < cameraIds_.size(); i++) { std::cout << "-- Measurement nr " << i << std::endl; std::cout << " pose1 key: " << keyFormatter(world_P_body_key_pairs_[i].first) << std::endl; @@ -409,14 +404,8 @@ class SmartProjectionPoseFactorRollingShutter * @return Cameras */ typename Base::Cameras cameras(const Values& values) const override { - size_t numViews = this->measured_.size(); - assert(numViews == K_all_.size()); - assert(numViews == alphas_.size()); - assert(numViews == body_P_sensors_.size()); - assert(numViews == world_P_body_key_pairs_.size()); - typename Base::Cameras cameras; - for (size_t i = 0; i < numViews; i++) { // for each measurement + for (size_t i = 0; i < this->measured_.size(); i++) { // for each measurement const Pose3& w_P_body1 = values.at(world_P_body_key_pairs_[i].first); const Pose3& w_P_body2 = diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index c4c670de3..6a3e31dd7 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -496,6 +496,91 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) { EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); } +/* *************************************************************************/ +TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) { + using namespace vanillaPoseRS; + + Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3; + + // create arbitrary body_T_sensor (transforms from sensor to body) + Pose3 body_T_sensor1 = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), + Point3(1, 1, 1)); + Pose3 body_T_sensor2 = Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), + Point3(0, 0, 1)); + Pose3 body_T_sensor3 = Pose3::identity(); + + Camera camera1(interp_pose1*body_T_sensor1, sharedK); + Camera camera2(interp_pose2*body_T_sensor2, sharedK); + Camera camera3(interp_pose3*body_T_sensor3, sharedK); + + // Project three landmarks into three cameras + projectToMultipleCameras(camera1, camera2, camera3, landmark1, measurements_lmk1); + projectToMultipleCameras(camera1, camera2, camera3, landmark2, measurements_lmk2); + projectToMultipleCameras(camera1, camera2, camera3, landmark3, measurements_lmk3); + + // create inputs + std::vector> 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)); + + std::vector interp_factors; + interp_factors.push_back(interp_factor1); + interp_factors.push_back(interp_factor2); + interp_factors.push_back(interp_factor3); + + Cameras cameraRig; + cameraRig.push_back(Camera(body_T_sensor1, sharedK)); + cameraRig.push_back(Camera(body_T_sensor2, sharedK)); + cameraRig.push_back(Camera(body_T_sensor3, sharedK)); + + SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, cameraRig)); + smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, {0,1,2}); + + SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, cameraRig)); + smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, {0,1,2}); + + SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, cameraRig)); + smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, {0,1,2}); + + 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); // pose above is the pose of the camera + 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-4)); +} + /* *************************************************************************/ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { // here we replicate a test in SmartProjectionPoseFactor by setting