diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index d58aea148..cf1526673 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -11,8 +11,7 @@ /** * @file SmartProjectionPoseFactorRollingShutter.h - * @brief Smart projection factor on poses modeling rolling shutter effect with - * given readout time + * @brief Smart projection factor on poses modeling rolling shutter effect with given readout time * @author Luca Carlone */ @@ -43,7 +42,10 @@ namespace gtsam { template class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor { - public: + + private: + typedef SmartProjectionFactor Base; + typedef SmartProjectionPoseFactorRollingShutter This; typedef typename CAMERA::CalibrationType CALIBRATION; protected: @@ -58,24 +60,23 @@ class SmartProjectionPoseFactorRollingShutter /// pair of consecutive poses std::vector alphas_; - /// Pose of the camera in the body frame - std::vector body_P_sensors_; + /// one or more cameras in the rig (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 + FastVector cameraIds_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - /// shorthand for base class type - typedef SmartProjectionFactor> Base; - - /// shorthand for this class - typedef SmartProjectionPoseFactorRollingShutter This; + typedef CAMERA Camera; + typedef CameraSet Cameras; /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - static const int DimBlock = - 12; ///< size of the variable stacking 2 poses from which the observation - ///< pose is interpolated + static const int DimBlock = 12; ///< size of the variable stacking 2 poses from which the observation + ///< pose is interpolated static const int DimPose = 6; ///< Pose3 dimension static const int ZDim = 2; ///< Measurement dimension (Point2) typedef Eigen::Matrix @@ -89,9 +90,30 @@ class SmartProjectionPoseFactorRollingShutter * @param params internal parameters of the smart factors */ SmartProjectionPoseFactorRollingShutter( - const SharedNoiseModel& sharedNoiseModel, + const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SmartProjectionParams& params = SmartProjectionParams()) - : Base(sharedNoiseModel, params) {} + : Base(sharedNoiseModel, params), + cameraRig_(cameraRig) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + } + + /** + * Constructor + * @param Isotropic measurement noise + * @param params internal parameters of the smart factors + */ + SmartProjectionPoseFactorRollingShutter( + const SharedNoiseModel& sharedNoiseModel, const Camera& camera, + const SmartProjectionParams& params = SmartProjectionParams()) + : Base(sharedNoiseModel, params) { + // use only configuration that works with this factor + Base::params_.degeneracyMode = gtsam::ZERO_ON_DEGENERACY; + Base::params_.linearizationMode = gtsam::HESSIAN; + cameraRig_.push_back(camera); + } + /** Virtual destructor */ ~SmartProjectionPoseFactorRollingShutter() override = default; @@ -112,8 +134,7 @@ class SmartProjectionPoseFactorRollingShutter */ void add(const Point2& measured, const Key& world_P_body_key1, const Key& world_P_body_key2, const double& alpha, - const boost::shared_ptr& K, - const Pose3& body_P_sensor = Pose3::identity()) { + const size_t cameraId = 0) { // store measurements in base class this->measured_.push_back(measured); @@ -133,11 +154,8 @@ class SmartProjectionPoseFactorRollingShutter // store interpolation factor alphas_.push_back(alpha); - // store fixed intrinsic calibration - K_all_.push_back(K); - - // store fixed extrinsics of the camera - body_P_sensors_.push_back(body_P_sensor); + // store id of the camera taking the measurement + cameraIds_.push_back(cameraId); } /** @@ -156,61 +174,39 @@ class SmartProjectionPoseFactorRollingShutter void add(const Point2Vector& measurements, const std::vector>& world_P_body_key_pairs, const std::vector& alphas, - const std::vector>& Ks, - const std::vector& body_P_sensors) { - assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == alphas.size()); - assert(world_P_body_key_pairs.size() == Ks.size()); + const FastVector& cameraIds = FastVector()) { + + if (world_P_body_key_pairs.size() != measurements.size() + || world_P_body_key_pairs.size() != alphas.size() + || world_P_body_key_pairs.size() != cameraIds.size()) { + throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " + "trying to add inconsistent inputs"); + } for (size_t i = 0; i < measurements.size(); i++) { add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, alphas[i], Ks[i], - body_P_sensors[i]); + world_P_body_key_pairs[i].second, alphas[i], + cameraIds.size() == 0 ? 0 : cameraIds[i]); // use 0 as default if cameraIds was not specified } } - /** - * Variant of the previous "add" function in which we include multiple - * measurements with the same (intrinsic and extrinsic) calibration - * @param measurements vector of the 2m dimensional location of the projection - * of a single landmark in the m views (the measurements) - * @param world_P_body_key_pairs vector where the i-th element contains a pair - * of keys corresponding to the pair of poses from which the observation pose - * 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 K (fixed) camera intrinsic calibration (same for all measurements) - * @param body_P_sensor (fixed) camera extrinsic calibration (same for all - * measurements) - */ - void add(const Point2Vector& measurements, - const std::vector>& world_P_body_key_pairs, - const std::vector& alphas, - const boost::shared_ptr& K, - const Pose3& body_P_sensor = Pose3::identity()) { - assert(world_P_body_key_pairs.size() == measurements.size()); - assert(world_P_body_key_pairs.size() == alphas.size()); - for (size_t i = 0; i < measurements.size(); i++) { - add(measurements[i], world_P_body_key_pairs[i].first, - world_P_body_key_pairs[i].second, alphas[i], K, body_P_sensor); - } - } - - /// return the calibration object - const std::vector>& calibration() const { - return K_all_; - } - /// return (for each observation) the keys of the pair of poses from which we /// interpolate - const std::vector>& world_P_body_key_pairs() const { + const std::vector> world_P_body_key_pairs() const { return world_P_body_key_pairs_; } /// return the interpolation factors alphas - const std::vector& alphas() const { return alphas_; } + const std::vector alphas() const { return alphas_; } - /// return the extrinsic camera calibration body_P_sensors - const std::vector& body_P_sensors() const { return body_P_sensors_; } + /// return the calibration object + inline Cameras cameraRig() const { + return cameraRig_; + } + + /// return the calibration object + inline FastVector cameraIds() const { + return cameraIds_; + } /** * print @@ -228,8 +224,8 @@ class SmartProjectionPoseFactorRollingShutter std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl; - body_P_sensors_[i].print("extrinsic calibration:\n"); - K_all_[i]->print("intrinsic calibration = "); + std::cout << "cameraId: " << cameraIds_[i] << std::endl; + cameraRig_[ cameraIds_[i] ].print("camera in rig:\n"); } Base::print("", keyFormatter); } @@ -237,8 +233,7 @@ class SmartProjectionPoseFactorRollingShutter /// equals bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { const SmartProjectionPoseFactorRollingShutter* e = - dynamic_cast*>( - &p); + dynamic_cast*>(&p); double keyPairsEqual = true; if (this->world_P_body_key_pairs_.size() == @@ -257,20 +252,9 @@ class SmartProjectionPoseFactorRollingShutter keyPairsEqual = false; } - double extrinsicCalibrationEqual = true; - if (this->body_P_sensors_.size() == e->body_P_sensors().size()) { - for (size_t i = 0; i < this->body_P_sensors_.size(); i++) { - if (!body_P_sensors_[i].equals(e->body_P_sensors()[i])) { - extrinsicCalibrationEqual = false; - break; - } - } - } else { - extrinsicCalibrationEqual = false; - } - - return e && Base::equals(p, tol) && K_all_ == e->calibration() && - alphas_ == e->alphas() && keyPairsEqual && extrinsicCalibrationEqual; + return e && Base::equals(p, tol) && alphas_ == e->alphas() && keyPairsEqual + && cameraRig_.equals(e->cameraRig()) + && std::equal(cameraIds_.begin(), cameraIds_.end(), e->cameraIds().begin()); } /** @@ -305,9 +289,9 @@ class SmartProjectionPoseFactorRollingShutter auto w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor, dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); - auto body_P_cam = body_P_sensors_[i]; + auto body_P_cam = cameraRig_[cameraIds_[i]].pose(); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); - PinholeCamera camera(w_P_cam, *K_all_[i]); + PinholeCamera camera(w_P_cam, cameraRig_[cameraIds_[i]].calibration()); // get jacobians and error vector for current measurement Point2 reprojectionError_i = @@ -434,9 +418,10 @@ class SmartProjectionPoseFactorRollingShutter double interpolationFactor = alphas_[i]; const Pose3& w_P_body = interpolate(w_P_body1, w_P_body2, interpolationFactor); - const Pose3& body_P_cam = body_P_sensors_[i]; + const Pose3& body_P_cam = cameraRig_[cameraIds_[i]].pose(); const Pose3& w_P_cam = w_P_body.compose(body_P_cam); - cameras.emplace_back(w_P_cam, K_all_[i]); + cameras.emplace_back(w_P_cam, make_shared( + cameraRig_[cameraIds_[i]].calibration())); } return cameras; } @@ -474,7 +459,6 @@ class SmartProjectionPoseFactorRollingShutter template void serialize(ARCHIVE& ar, const unsigned int /*version*/) { ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); - ar& BOOST_SERIALIZATION_NVP(K_all_); } }; // end of class declaration diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index 0b94d2c3f..6c28cbda5 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -61,10 +61,13 @@ static double interp_factor1 = 0.3; static double interp_factor2 = 0.4; static double interp_factor3 = 0.5; +static size_t cameraId1 = 0; + /* ************************************************************************* */ // default Cal3_S2 poses with rolling shutter effect namespace vanillaPoseRS { typedef PinholePose Camera; +typedef CameraSet Cameras; static Cal3_S2::shared_ptr sharedK(new Cal3_S2(fov, w, h)); Pose3 interp_pose1 = interpolate(level_pose, pose_right, interp_factor1); Pose3 interp_pose2 = interpolate(pose_right, pose_above, interp_factor2); @@ -80,21 +83,23 @@ typedef SmartProjectionPoseFactorRollingShutter> /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); + using namespace vanillaPoseRS; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { + using namespace vanillaPoseRS; SmartProjectionParams params; params.setRankTolerance(rankTol); - SmartFactorRS factor1(model, params); + SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); } /* ************************************************************************* */ TEST(SmartProjectionPoseFactorRollingShutter, add) { - using namespace vanillaPose; - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor, sharedK, body_P_sensor); + using namespace vanillaPoseRS; + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, Camera(Pose3::identity(), sharedK))); + factor1->add(measurement1, x1, x2, interp_factor); } /* ************************************************************************* */ @@ -112,1030 +117,1039 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) { key_pairs.push_back(std::make_pair(x2, x3)); key_pairs.push_back(std::make_pair(x3, x4)); - std::vector> intrinsicCalibrations; - intrinsicCalibrations.push_back(sharedK); - intrinsicCalibrations.push_back(sharedK); - intrinsicCalibrations.push_back(sharedK); - - std::vector extrinsicCalibrations; - extrinsicCalibrations.push_back(body_P_sensor); - extrinsicCalibrations.push_back(body_P_sensor); - extrinsicCalibrations.push_back(body_P_sensor); - std::vector interp_factors; interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor3); + std::vector cameraIds{0, 0, 0}; + + Cameras cameraRig; + cameraRig.push_back( Camera(body_P_sensor, sharedK) ); + // create by adding a batch of measurements with a bunch of calibrations - SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model)); - factor2->add(measurements, key_pairs, interp_factors, intrinsicCalibrations, - extrinsicCalibrations); + SmartFactorRS::shared_ptr factor2(new SmartFactorRS(model, cameraRig)); + factor2->add(measurements, key_pairs, interp_factors, cameraIds); // create by adding a batch of measurements with a single calibrations - SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model)); - factor3->add(measurements, key_pairs, interp_factors, sharedK, body_P_sensor); + SmartFactorRS::shared_ptr factor3(new SmartFactorRS(model, cameraRig)); + factor3->add(measurements, key_pairs, interp_factors, cameraIds); - { // create equal factors and show equal returns true - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, body_P_sensor); - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - - EXPECT(factor1->equals(*factor2)); - EXPECT(factor1->equals(*factor3)); - } - { // create slightly different factors (different keys) and show equal - // returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x2, interp_factor2, sharedK, - body_P_sensor); // different! - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); - - EXPECT(!factor1->equals(*factor2)); - EXPECT(!factor1->equals(*factor3)); - } +// { // create equal factors and show equal returns true +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); +// factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); +// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create equal factors and show equal returns true (use default cameraId) +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1); +// factor1->add(measurement2, x2, x3, interp_factor2); +// factor1->add(measurement3, x3, x4, interp_factor3); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create equal factors and show equal returns true (use default cameraId) +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurements, key_pairs, interp_factors); +// +// EXPECT(factor1->equals(*factor2)); +// EXPECT(factor1->equals(*factor3)); +// } +// { // create slightly different factors (different keys) and show equal +// // returns false +// SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); +// factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); +// factor1->add(measurement2, x2, x2, interp_factor2, cameraId1); // different! +// factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); +// +// EXPECT(!factor1->equals(*factor2)); +// EXPECT(!factor1->equals(*factor3)); +// } { // create slightly different factors (different extrinsics) and show equal // returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor2, sharedK, - body_P_sensor * body_P_sensor); // different! - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + Cameras cameraRig2; + cameraRig2.push_back( Camera(body_P_sensor * body_P_sensor, sharedK) ); + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig2)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor2, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } { // create slightly different factors (different interp factors) and show // equal returns false - SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); - factor1->add(measurement1, x1, x2, interp_factor1, sharedK, body_P_sensor); - factor1->add(measurement2, x2, x3, interp_factor1, sharedK, - body_P_sensor); // different! - factor1->add(measurement3, x3, x4, interp_factor3, sharedK, body_P_sensor); + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model, cameraRig)); + factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); + factor1->add(measurement2, x2, x3, interp_factor1, cameraId1); // different! + factor1->add(measurement3, x3, x4, interp_factor3, cameraId1); EXPECT(!factor1->equals(*factor2)); EXPECT(!factor1->equals(*factor3)); } } -static const int DimBlock = 12; ///< size of the variable stacking 2 poses from - ///< which the observation pose is interpolated -static const int ZDim = 2; ///< Measurement dimension (Point2) -typedef Eigen::Matrix - MatrixZD; // F blocks (derivatives wrt camera) -typedef std::vector> - FBlocks; // vector of F blocks - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { - using namespace vanillaPoseRS; - - // Project two landmarks into two cameras - Point2 level_uv = cam1.project(landmark1); - Point2 level_uv_right = cam2.project(landmark1); - Pose3 body_P_sensorId = Pose3::identity(); - - SmartFactorRS factor(model); - factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above); - - double actualError = factor.error(values); - double expectedError = 0.0; - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); - - // Check triangulation - factor.triangulateSafe(factor.cameras(values)); - TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - EXPECT(assert_equal( - landmark1, - *point)); // check triangulation result matches expected 3D landmark - - // Check Jacobians - // -- actual Jacobians - FBlocks actualFs; - Matrix actualE; - Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, - values); - EXPECT(actualE.rows() == 4); - EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); - EXPECT(actualb.cols() == 1); - EXPECT(actualFs.size() == 2); - - // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, - x2, l0, sharedK, body_P_sensorId); - Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = factor1.evaluateError( - level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); - EXPECT( - assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, - x2, x3, l0, sharedK, body_P_sensorId); - Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = factor2.evaluateError( - pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); - EXPECT( - assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { - // also includes non-identical extrinsic calibration - using namespace vanillaPoseRS; - - // Project two landmarks into two cameras - Point2 pixelError(0.5, 1.0); - Point2 level_uv = cam1.project(landmark1) + pixelError; - Point2 level_uv_right = cam2.project(landmark1); - Pose3 body_P_sensorNonId = body_P_sensor; - - SmartFactorRS factor(model); - factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); - factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, - body_P_sensorNonId); - - Values values; // it's a pose factor, hence these are poses - values.insert(x1, level_pose); - values.insert(x2, pose_right); - values.insert(x3, pose_above); - - // Perform triangulation - factor.triangulateSafe(factor.cameras(values)); - TriangulationResult point = factor.point(); - EXPECT(point.valid()); // check triangulated point is valid - Point3 landmarkNoisy = *point; - - // Check Jacobians - // -- actual Jacobians - FBlocks actualFs; - Matrix actualE; - Vector actualb; - factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, - values); - EXPECT(actualE.rows() == 4); - EXPECT(actualE.cols() == 3); - EXPECT(actualb.rows() == 4); - EXPECT(actualb.cols() == 1); - EXPECT(actualFs.size() == 2); - - // -- expected Jacobians from ProjectionFactorsRollingShutter - ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, - x2, l0, sharedK, body_P_sensorNonId); - Matrix expectedF11, expectedF12, expectedE1; - Vector expectedb1 = - factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, - expectedF12, expectedE1); - EXPECT( - assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); - - ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, - x2, x3, l0, sharedK, - body_P_sensorNonId); - Matrix expectedF21, expectedF22, expectedE2; - Vector expectedb2 = - factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, - expectedF22, expectedE2); - EXPECT( - assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); - EXPECT( - assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); - EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); - // by definition computeJacobiansWithTriangulatedPoint returns minus - // reprojectionError - EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); - - // Check errors - double actualError = factor.error(values); // from smart factor - NonlinearFactorGraph nfg; - nfg.add(factor1); - nfg.add(factor2); - values.insert(l0, landmarkNoisy); - double expectedError = nfg.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { - using namespace vanillaPoseRS; - 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> 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); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - 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-6)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { - // here we replicate a test in SmartProjectionPoseFactor by setting - // interpolation factors to 0 and 1 (such that the rollingShutter measurements - // falls back to standard pixel measurements) Note: this is a quite extreme - // test since in typical camera you would not have more than 1 measurement per - // landmark at each interpolated pose - 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)); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - double interp_factor = 0; // equivalent to measurement taken at pose 1 - smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); - interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, - body_P_sensorId); - - SmartFactor::Cameras cameras; - cameras.push_back(cam1); - cameras.push_back(cam2); - - // Make sure triangulation works - EXPECT(smartFactor1->triangulateSafe(cameras)); - EXPECT(!smartFactor1->isDegenerate()); - EXPECT(!smartFactor1->isPointBehindCamera()); - boost::optional p = smartFactor1->point(); - EXPECT(p); - EXPECT(assert_equal(landmark1, *p)); - - VectorValues zeroDelta; - Vector6 delta; - delta.setZero(); - zeroDelta.insert(x1, delta); - zeroDelta.insert(x2, delta); - - VectorValues perturbedDelta; - delta.setOnes(); - perturbedDelta.insert(x1, delta); - perturbedDelta.insert(x2, delta); - double expectedError = 2500; - - // After eliminating the point, A1 and A2 contain 2-rank information on - // cameras: - Matrix16 A1, A2; - A1 << -10, 0, 0, 0, 1, 0; - A2 << 10, 0, 1, 0, -1, 0; - A1 *= 10. / sigma; - A2 *= 10. / sigma; - Matrix expectedInformation; // filled below - - // createHessianFactor - Matrix66 G11 = 0.5 * A1.transpose() * A1; - Matrix66 G12 = 0.5 * A1.transpose() * A2; - Matrix66 G22 = 0.5 * A2.transpose() * A2; - - Vector6 g1; - g1.setZero(); - Vector6 g2; - g2.setZero(); - - double f = 0; - - RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); - expectedInformation = expected.information(); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - boost::shared_ptr> actual = - smartFactor1->createHessianFactor(values); - EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); - EXPECT(assert_equal(expected, *actual, 1e-6)); - EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); - EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { - using namespace vanillaPoseRS; - 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> 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); - - double excludeLandmarksFutherThanDist = 1e10; // very large - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(true); - - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor3(model, params); - smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - 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); - - 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); - - // Optimization should correct 3rd pose - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - optimization_3poses_landmarkDistance) { - using namespace vanillaPoseRS; - 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> 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); - - double excludeLandmarksFutherThanDist = 2; - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setEnableEPI(false); - - SmartFactorRS smartFactor1(model, params); - smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor2(model, params); - smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS smartFactor3(model, params); - smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - 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); - - 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); - - // All factors are disabled (due to the distance threshold) and pose should - // remain where it is - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(values.at(x3), result.at(x3))); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - optimization_3poses_dynamicOutlierRejection) { - using namespace vanillaPoseRS; - // add fourth landmark - Point3 landmark4(5, -0.5, 1); - - Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, - measurements_lmk4; - // Project 4 landmarks into cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); - projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); - measurements_lmk4.at(0) = - measurements_lmk4.at(0) + Point2(10, 10); // add outlier - - // 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); - - double excludeLandmarksFutherThanDist = 1e10; - double dynamicOutlierRejectionThreshold = - 3; // max 3 pixel of average reprojection error - - SmartProjectionParams params; - params.setRankTolerance(1.0); - params.setLinearizationMode(gtsam::HESSIAN); - params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); - params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); - params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); - params.setEnableEPI(false); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); - smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(smartFactor4); - graph.addPrior(x1, level_pose, noisePrior); - graph.addPrior(x2, pose_right, noisePrior); - - Pose3 noise_pose = Pose3( - Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.01, 0.01, - 0.01)); // smaller noise, otherwise outlier rejection will kick in - 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); - - // Optimization should correct 3rd pose - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); - result = optimizer.optimize(); - EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - hessianComparedToProjFactorsRollingShutter) { - using namespace vanillaPoseRS; - Point2Vector measurements_lmk1; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); - - // 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); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); - - 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 the factor to calculate the Jacobians - Matrix F = Matrix::Zero(2 * 3, 6 * 3); - Matrix E = Matrix::Zero(2 * 3, 3); - Vector b = Vector::Zero(6); - - // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, - model, x1, x2, l0, sharedK); - Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian - // computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(0, 0) = H1Actual; - F.block<2, 6>(0, 6) = H2Actual; - E.block<2, 3>(0, 0) = H3Actual; - - ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, - model, x2, x3, l0, sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(2, 6) = H1Actual; - F.block<2, 6>(2, 12) = H2Actual; - E.block<2, 3>(2, 0) = H3Actual; - - ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, - model, x3, x1, l0, sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(4, 12) = H1Actual; - F.block<2, 6>(4, 0) = H2Actual; - E.block<2, 3>(4, 0) = H3Actual; - - // 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_projFactorsRS; - nfg_projFactorsRS.add(factor11); - nfg_projFactorsRS.add(factor12); - nfg_projFactorsRS.add(factor13); - values.insert(l0, *point); - - double actualError = smartFactor1->error(values); - double expectedError = nfg_projFactorsRS.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - hessianComparedToProjFactorsRollingShutter_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 vanillaPoseRS; - 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> 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)); - key_pairs.push_back(std::make_pair(x1, x2)); - - std::vector interp_factors; - interp_factors.push_back(interp_factor1); - interp_factors.push_back(interp_factor2); - interp_factors.push_back(interp_factor3); - interp_factors.push_back(interp_factor1); - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, - sharedK); - - 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 ProjectionFactorRollingShutter factor to calculate the - // Jacobians - Matrix F = Matrix::Zero(2 * 4, 6 * 3); - Matrix E = Matrix::Zero(2 * 4, 3); - Vector b = Vector::Zero(8); - - // create projection factors rolling shutter - ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], - interp_factor1, model, x1, x2, l0, - sharedK); - Matrix H1Actual, H2Actual, H3Actual; - // note: b is minus the reprojection error, cf the smart factor jacobian - // computation - b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(0, 0) = H1Actual; - F.block<2, 6>(0, 6) = H2Actual; - E.block<2, 3>(0, 0) = H3Actual; - - ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], - interp_factor2, model, x2, x3, l0, - sharedK); - b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(2, 6) = H1Actual; - F.block<2, 6>(2, 12) = H2Actual; - E.block<2, 3>(2, 0) = H3Actual; - - ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], - interp_factor3, model, x3, x1, l0, - sharedK); - b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(4, 12) = H1Actual; - F.block<2, 6>(4, 0) = H2Actual; - E.block<2, 3>(4, 0) = H3Actual; - - ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], - interp_factor1, model, x1, x2, l0, - sharedK); - b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, - H2Actual, H3Actual); - F.block<2, 6>(6, 0) = H1Actual; - F.block<2, 6>(6, 6) = H2Actual; - E.block<2, 3>(6, 0) = H3Actual; - - // 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_projFactorsRS; - nfg_projFactorsRS.add(factor11); - nfg_projFactorsRS.add(factor12); - nfg_projFactorsRS.add(factor13); - nfg_projFactorsRS.add(factor14); - values.insert(l0, *point); - - double actualError = smartFactor1->error(values); - double expectedError = nfg_projFactorsRS.error(values); - EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); -} - -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, - optimization_3poses_measurementsFromSamePose) { - using namespace vanillaPoseRS; - 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> 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); - - // 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> key_pairs_redundant = key_pairs; - key_pairs_redundant.push_back( - key_pairs.at(0)); // we readd the first pair of keys - std::vector interp_factors_redundant = interp_factors; - interp_factors_redundant.push_back( - interp_factors.at(0)); // we readd the first interp factor - - SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); - smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, - interp_factors_redundant, sharedK); - - SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); - smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); - - SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); - smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); - - 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 -// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) -//| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: -// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 -// children, min: 0 max: 0) -/* *************************************************************************/ -TEST(SmartProjectionPoseFactorRollingShutter, 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; i < nrTests; i++) { - SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); - double interp_factor = 0; // equivalent to measurement taken at pose 1 - smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, - sharedKSimple, body_P_sensorId); - interp_factor = 1; // equivalent to measurement taken at pose 2 - smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, - sharedKSimple, body_P_sensorId); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - gttic_(SF_RS_LINEARIZE); - smartFactorRS->linearize(values); - gttoc_(SF_RS_LINEARIZE); - } - - for (size_t i = 0; i < nrTests; i++) { - SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); - smartFactor->add(measurements_lmk1[0], x1); - smartFactor->add(measurements_lmk1[1], x2); - - Values values; - values.insert(x1, pose1); - values.insert(x2, pose2); - gttic_(RS_LINEARIZE); - smartFactor->linearize(values); - gttoc_(RS_LINEARIZE); - } - tictoc_print_(); -} -#endif +//static const int DimBlock = 12; ///< size of the variable stacking 2 poses from +// ///< which the observation pose is interpolated +//static const int ZDim = 2; ///< Measurement dimension (Point2) +//typedef Eigen::Matrix +// MatrixZD; // F blocks (derivatives wrt camera) +//typedef std::vector> +// FBlocks; // vector of F blocks +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) { +// using namespace vanillaPoseRS; +// +// // Project two landmarks into two cameras +// Point2 level_uv = cam1.project(landmark1); +// Point2 level_uv_right = cam2.project(landmark1); +// Pose3 body_P_sensorId = Pose3::identity(); +// +// SmartFactorRS factor(model); +// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorId); +// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, body_P_sensorId); +// +// Values values; // it's a pose factor, hence these are poses +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// values.insert(x3, pose_above); +// +// double actualError = factor.error(values); +// double expectedError = 0.0; +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +// +// // Check triangulation +// factor.triangulateSafe(factor.cameras(values)); +// TriangulationResult point = factor.point(); +// EXPECT(point.valid()); // check triangulated point is valid +// EXPECT(assert_equal( +// landmark1, +// *point)); // check triangulation result matches expected 3D landmark +// +// // Check Jacobians +// // -- actual Jacobians +// FBlocks actualFs; +// Matrix actualE; +// Vector actualb; +// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, +// values); +// EXPECT(actualE.rows() == 4); +// EXPECT(actualE.cols() == 3); +// EXPECT(actualb.rows() == 4); +// EXPECT(actualb.cols() == 1); +// EXPECT(actualFs.size() == 2); +// +// // -- expected Jacobians from ProjectionFactorsRollingShutter +// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, +// x2, l0, sharedK, body_P_sensorId); +// Matrix expectedF11, expectedF12, expectedE1; +// Vector expectedb1 = factor1.evaluateError( +// level_pose, pose_right, landmark1, expectedF11, expectedF12, expectedE1); +// EXPECT( +// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); +// +// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, +// x2, x3, l0, sharedK, body_P_sensorId); +// Matrix expectedF21, expectedF22, expectedE2; +// Vector expectedb2 = factor2.evaluateError( +// pose_right, pose_above, landmark1, expectedF21, expectedF22, expectedE2); +// EXPECT( +// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) { +// // also includes non-identical extrinsic calibration +// using namespace vanillaPoseRS; +// +// // Project two landmarks into two cameras +// Point2 pixelError(0.5, 1.0); +// Point2 level_uv = cam1.project(landmark1) + pixelError; +// Point2 level_uv_right = cam2.project(landmark1); +// Pose3 body_P_sensorNonId = body_P_sensor; +// +// SmartFactorRS factor(model); +// factor.add(level_uv, x1, x2, interp_factor1, sharedK, body_P_sensorNonId); +// factor.add(level_uv_right, x2, x3, interp_factor2, sharedK, +// body_P_sensorNonId); +// +// Values values; // it's a pose factor, hence these are poses +// values.insert(x1, level_pose); +// values.insert(x2, pose_right); +// values.insert(x3, pose_above); +// +// // Perform triangulation +// factor.triangulateSafe(factor.cameras(values)); +// TriangulationResult point = factor.point(); +// EXPECT(point.valid()); // check triangulated point is valid +// Point3 landmarkNoisy = *point; +// +// // Check Jacobians +// // -- actual Jacobians +// FBlocks actualFs; +// Matrix actualE; +// Vector actualb; +// factor.computeJacobiansWithTriangulatedPoint(actualFs, actualE, actualb, +// values); +// EXPECT(actualE.rows() == 4); +// EXPECT(actualE.cols() == 3); +// EXPECT(actualb.rows() == 4); +// EXPECT(actualb.cols() == 1); +// EXPECT(actualFs.size() == 2); +// +// // -- expected Jacobians from ProjectionFactorsRollingShutter +// ProjectionFactorRollingShutter factor1(level_uv, interp_factor1, model, x1, +// x2, l0, sharedK, body_P_sensorNonId); +// Matrix expectedF11, expectedF12, expectedE1; +// Vector expectedb1 = +// factor1.evaluateError(level_pose, pose_right, landmarkNoisy, expectedF11, +// expectedF12, expectedE1); +// EXPECT( +// assert_equal(expectedF11, Matrix(actualFs[0].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF12, Matrix(actualFs[0].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE1, Matrix(actualE.block(0, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb1, -Vector(actualb.segment<2>(0)), 1e-5)); +// +// ProjectionFactorRollingShutter factor2(level_uv_right, interp_factor2, model, +// x2, x3, l0, sharedK, +// body_P_sensorNonId); +// Matrix expectedF21, expectedF22, expectedE2; +// Vector expectedb2 = +// factor2.evaluateError(pose_right, pose_above, landmarkNoisy, expectedF21, +// expectedF22, expectedE2); +// EXPECT( +// assert_equal(expectedF21, Matrix(actualFs[1].block(0, 0, 2, 6)), 1e-5)); +// EXPECT( +// assert_equal(expectedF22, Matrix(actualFs[1].block(0, 6, 2, 6)), 1e-5)); +// EXPECT(assert_equal(expectedE2, Matrix(actualE.block(2, 0, 2, 3)), 1e-5)); +// // by definition computeJacobiansWithTriangulatedPoint returns minus +// // reprojectionError +// EXPECT(assert_equal(expectedb2, -Vector(actualb.segment<2>(2)), 1e-5)); +// +// // Check errors +// double actualError = factor.error(values); // from smart factor +// NonlinearFactorGraph nfg; +// nfg.add(factor1); +// nfg.add(factor2); +// values.insert(l0, landmarkNoisy); +// double expectedError = nfg.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) { +// using namespace vanillaPoseRS; +// 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> 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); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); +// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); +// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// 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-6)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) { +// // here we replicate a test in SmartProjectionPoseFactor by setting +// // interpolation factors to 0 and 1 (such that the rollingShutter measurements +// // falls back to standard pixel measurements) Note: this is a quite extreme +// // test since in typical camera you would not have more than 1 measurement per +// // landmark at each interpolated pose +// 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)); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// double interp_factor = 0; // equivalent to measurement taken at pose 1 +// smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor, sharedKSimple, +// body_P_sensorId); +// interp_factor = 1; // equivalent to measurement taken at pose 2 +// smartFactor1->add(measurements_lmk1[1], x1, x2, interp_factor, sharedKSimple, +// body_P_sensorId); +// +// SmartFactor::Cameras cameras; +// cameras.push_back(cam1); +// cameras.push_back(cam2); +// +// // Make sure triangulation works +// EXPECT(smartFactor1->triangulateSafe(cameras)); +// EXPECT(!smartFactor1->isDegenerate()); +// EXPECT(!smartFactor1->isPointBehindCamera()); +// boost::optional p = smartFactor1->point(); +// EXPECT(p); +// EXPECT(assert_equal(landmark1, *p)); +// +// VectorValues zeroDelta; +// Vector6 delta; +// delta.setZero(); +// zeroDelta.insert(x1, delta); +// zeroDelta.insert(x2, delta); +// +// VectorValues perturbedDelta; +// delta.setOnes(); +// perturbedDelta.insert(x1, delta); +// perturbedDelta.insert(x2, delta); +// double expectedError = 2500; +// +// // After eliminating the point, A1 and A2 contain 2-rank information on +// // cameras: +// Matrix16 A1, A2; +// A1 << -10, 0, 0, 0, 1, 0; +// A2 << 10, 0, 1, 0, -1, 0; +// A1 *= 10. / sigma; +// A2 *= 10. / sigma; +// Matrix expectedInformation; // filled below +// +// // createHessianFactor +// Matrix66 G11 = 0.5 * A1.transpose() * A1; +// Matrix66 G12 = 0.5 * A1.transpose() * A2; +// Matrix66 G22 = 0.5 * A2.transpose() * A2; +// +// Vector6 g1; +// g1.setZero(); +// Vector6 g2; +// g2.setZero(); +// +// double f = 0; +// +// RegularHessianFactor<6> expected(x1, x2, G11, G12, g1, G22, g2, f); +// expectedInformation = expected.information(); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// boost::shared_ptr> actual = +// smartFactor1->createHessianFactor(values); +// EXPECT(assert_equal(expectedInformation, actual->information(), 1e-6)); +// EXPECT(assert_equal(expected, *actual, 1e-6)); +// EXPECT_DOUBLES_EQUAL(0, actual->error(zeroDelta), 1e-6); +// EXPECT_DOUBLES_EQUAL(expectedError, actual->error(perturbedDelta), 1e-6); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) { +// using namespace vanillaPoseRS; +// 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> 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); +// +// double excludeLandmarksFutherThanDist = 1e10; // very large +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(true); +// +// SmartFactorRS smartFactor1(model, params); +// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor2(model, params); +// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor3(model, params); +// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// 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); +// +// 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); +// +// // Optimization should correct 3rd pose +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// optimization_3poses_landmarkDistance) { +// using namespace vanillaPoseRS; +// 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> 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); +// +// double excludeLandmarksFutherThanDist = 2; +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setEnableEPI(false); +// +// SmartFactorRS smartFactor1(model, params); +// smartFactor1.add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor2(model, params); +// smartFactor2.add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS smartFactor3(model, params); +// smartFactor3.add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// 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); +// +// 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); +// +// // All factors are disabled (due to the distance threshold) and pose should +// // remain where it is +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(values.at(x3), result.at(x3))); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// optimization_3poses_dynamicOutlierRejection) { +// using namespace vanillaPoseRS; +// // add fourth landmark +// Point3 landmark4(5, -0.5, 1); +// +// Point2Vector measurements_lmk1, measurements_lmk2, measurements_lmk3, +// measurements_lmk4; +// // Project 4 landmarks into cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_lmk2); +// projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_lmk3); +// projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_lmk4); +// measurements_lmk4.at(0) = +// measurements_lmk4.at(0) + Point2(10, 10); // add outlier +// +// // 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); +// +// double excludeLandmarksFutherThanDist = 1e10; +// double dynamicOutlierRejectionThreshold = +// 3; // max 3 pixel of average reprojection error +// +// SmartProjectionParams params; +// params.setRankTolerance(1.0); +// params.setLinearizationMode(gtsam::HESSIAN); +// params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); +// params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); +// params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); +// params.setEnableEPI(false); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model, params)); +// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model, params)); +// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model, params)); +// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor4(new SmartFactorRS(model, params)); +// smartFactor4->add(measurements_lmk4, key_pairs, interp_factors, sharedK); +// +// const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); +// +// NonlinearFactorGraph graph; +// graph.push_back(smartFactor1); +// graph.push_back(smartFactor2); +// graph.push_back(smartFactor3); +// graph.push_back(smartFactor4); +// graph.addPrior(x1, level_pose, noisePrior); +// graph.addPrior(x2, pose_right, noisePrior); +// +// Pose3 noise_pose = Pose3( +// Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), +// Point3(0.01, 0.01, +// 0.01)); // smaller noise, otherwise outlier rejection will kick in +// 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); +// +// // Optimization should correct 3rd pose +// Values result; +// LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); +// result = optimizer.optimize(); +// EXPECT(assert_equal(pose_above, result.at(x3), 1e-6)); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// hessianComparedToProjFactorsRollingShutter) { +// using namespace vanillaPoseRS; +// Point2Vector measurements_lmk1; +// +// // Project three landmarks into three cameras +// projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_lmk1); +// +// // 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); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1, key_pairs, interp_factors, sharedK); +// +// 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 the factor to calculate the Jacobians +// Matrix F = Matrix::Zero(2 * 3, 6 * 3); +// Matrix E = Matrix::Zero(2 * 3, 3); +// Vector b = Vector::Zero(6); +// +// // create projection factors rolling shutter +// ProjectionFactorRollingShutter factor11(measurements_lmk1[0], interp_factor1, +// model, x1, x2, l0, sharedK); +// Matrix H1Actual, H2Actual, H3Actual; +// // note: b is minus the reprojection error, cf the smart factor jacobian +// // computation +// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(0, 0) = H1Actual; +// F.block<2, 6>(0, 6) = H2Actual; +// E.block<2, 3>(0, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor12(measurements_lmk1[1], interp_factor2, +// model, x2, x3, l0, sharedK); +// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(2, 6) = H1Actual; +// F.block<2, 6>(2, 12) = H2Actual; +// E.block<2, 3>(2, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor13(measurements_lmk1[2], interp_factor3, +// model, x3, x1, l0, sharedK); +// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(4, 12) = H1Actual; +// F.block<2, 6>(4, 0) = H2Actual; +// E.block<2, 3>(4, 0) = H3Actual; +// +// // 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_projFactorsRS; +// nfg_projFactorsRS.add(factor11); +// nfg_projFactorsRS.add(factor12); +// nfg_projFactorsRS.add(factor13); +// values.insert(l0, *point); +// +// double actualError = smartFactor1->error(values); +// double expectedError = nfg_projFactorsRS.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// hessianComparedToProjFactorsRollingShutter_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 vanillaPoseRS; +// 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> 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)); +// key_pairs.push_back(std::make_pair(x1, x2)); +// +// std::vector interp_factors; +// interp_factors.push_back(interp_factor1); +// interp_factors.push_back(interp_factor2); +// interp_factors.push_back(interp_factor3); +// interp_factors.push_back(interp_factor1); +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors, +// sharedK); +// +// 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 ProjectionFactorRollingShutter factor to calculate the +// // Jacobians +// Matrix F = Matrix::Zero(2 * 4, 6 * 3); +// Matrix E = Matrix::Zero(2 * 4, 3); +// Vector b = Vector::Zero(8); +// +// // create projection factors rolling shutter +// ProjectionFactorRollingShutter factor11(measurements_lmk1_redundant[0], +// interp_factor1, model, x1, x2, l0, +// sharedK); +// Matrix H1Actual, H2Actual, H3Actual; +// // note: b is minus the reprojection error, cf the smart factor jacobian +// // computation +// b.segment<2>(0) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(0, 0) = H1Actual; +// F.block<2, 6>(0, 6) = H2Actual; +// E.block<2, 3>(0, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor12(measurements_lmk1_redundant[1], +// interp_factor2, model, x2, x3, l0, +// sharedK); +// b.segment<2>(2) = -factor12.evaluateError(pose2, pose3, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(2, 6) = H1Actual; +// F.block<2, 6>(2, 12) = H2Actual; +// E.block<2, 3>(2, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor13(measurements_lmk1_redundant[2], +// interp_factor3, model, x3, x1, l0, +// sharedK); +// b.segment<2>(4) = -factor13.evaluateError(pose3, pose1, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(4, 12) = H1Actual; +// F.block<2, 6>(4, 0) = H2Actual; +// E.block<2, 3>(4, 0) = H3Actual; +// +// ProjectionFactorRollingShutter factor14(measurements_lmk1_redundant[3], +// interp_factor1, model, x1, x2, l0, +// sharedK); +// b.segment<2>(6) = -factor11.evaluateError(pose1, pose2, *point, H1Actual, +// H2Actual, H3Actual); +// F.block<2, 6>(6, 0) = H1Actual; +// F.block<2, 6>(6, 6) = H2Actual; +// E.block<2, 3>(6, 0) = H3Actual; +// +// // 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_projFactorsRS; +// nfg_projFactorsRS.add(factor11); +// nfg_projFactorsRS.add(factor12); +// nfg_projFactorsRS.add(factor13); +// nfg_projFactorsRS.add(factor14); +// values.insert(l0, *point); +// +// double actualError = smartFactor1->error(values); +// double expectedError = nfg_projFactorsRS.error(values); +// EXPECT_DOUBLES_EQUAL(expectedError, actualError, 1e-7); +//} +// +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, +// optimization_3poses_measurementsFromSamePose) { +// using namespace vanillaPoseRS; +// 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> 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); +// +// // 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> key_pairs_redundant = key_pairs; +// key_pairs_redundant.push_back( +// key_pairs.at(0)); // we readd the first pair of keys +// std::vector interp_factors_redundant = interp_factors; +// interp_factors_redundant.push_back( +// interp_factors.at(0)); // we readd the first interp factor +// +// SmartFactorRS::shared_ptr smartFactor1(new SmartFactorRS(model)); +// smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant, +// interp_factors_redundant, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor2(new SmartFactorRS(model)); +// smartFactor2->add(measurements_lmk2, key_pairs, interp_factors, sharedK); +// +// SmartFactorRS::shared_ptr smartFactor3(new SmartFactorRS(model)); +// smartFactor3->add(measurements_lmk3, key_pairs, interp_factors, sharedK); +// +// 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 +//// -Total: 0 CPU (0 times, 0 wall, 0.04 children, min: 0 max: 0) +////| -SF RS LINEARIZE: 0.02 CPU (1000 times, 0.017244 wall, 0.02 children, min: +//// 0 max: 0) | -RS LINEARIZE: 0.02 CPU (1000 times, 0.009035 wall, 0.02 +//// children, min: 0 max: 0) +///* *************************************************************************/ +//TEST(SmartProjectionPoseFactorRollingShutter, 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; i < nrTests; i++) { +// SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(model)); +// double interp_factor = 0; // equivalent to measurement taken at pose 1 +// smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor, +// sharedKSimple, body_P_sensorId); +// interp_factor = 1; // equivalent to measurement taken at pose 2 +// smartFactorRS->add(measurements_lmk1[1], x1, x2, interp_factor, +// sharedKSimple, body_P_sensorId); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(SF_RS_LINEARIZE); +// smartFactorRS->linearize(values); +// gttoc_(SF_RS_LINEARIZE); +// } +// +// for (size_t i = 0; i < nrTests; i++) { +// SmartFactor::shared_ptr smartFactor(new SmartFactor(model, sharedKSimple)); +// smartFactor->add(measurements_lmk1[0], x1); +// smartFactor->add(measurements_lmk1[1], x2); +// +// Values values; +// values.insert(x1, pose1); +// values.insert(x2, pose2); +// gttic_(RS_LINEARIZE); +// smartFactor->linearize(values); +// gttoc_(RS_LINEARIZE); +// } +// tictoc_print_(); +//} +//#endif /* ************************************************************************* */ int main() {