diff --git a/gtsam/base/Lie.h b/gtsam/base/Lie.h index 250ee6bcf..c93456a28 100644 --- a/gtsam/base/Lie.h +++ b/gtsam/base/Lie.h @@ -17,7 +17,7 @@ * @author Frank Dellaert * @author Mike Bosse * @author Duy Nguyen Ta - * @author shteren1 + * @author Yotam Stern */ diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index c7f220c10..997c33846 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -50,7 +50,7 @@ static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): -// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +// lmParams.verbosityLM = LevenbergMarquardtParams::SUMMARY; /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor) { diff --git a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h index f1f0e2d71..13ba4e85e 100644 --- a/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h +++ b/gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h @@ -167,12 +167,17 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< return K_all_; } + /// return (for each observation) the key of the pair of poses from which we interpolate + const std::vector> world_P_body_key_pairs() const { + return world_P_body_key_pairs_; + } + /// return the interpolation factors gammas const std::vector getGammas() const { return gammas_; } - /// return the interpolation factors gammas + /// return the extrinsic camera calibration body_P_sensors const std::vector body_P_sensors() const { return body_P_sensors_; } @@ -192,7 +197,8 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< std::cout << " pose2 key: " << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; std::cout << " gamma: " << gammas_[i] << std::endl; - K_all_[i]->print("calibration = "); + body_P_sensors_[i].print("extrinsic calibration:\n"); + K_all_[i]->print("intrinsic calibration = "); } Base::print("", keyFormatter); } @@ -202,8 +208,30 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< const SmartProjectionPoseFactorRollingShutter* e = dynamic_cast*>(&p); + double keyPairsEqual = true; + if(this->world_P_body_key_pairs_.size() == e->world_P_body_key_pairs().size()){ + for(size_t k=0; k< this->world_P_body_key_pairs_.size(); k++){ + const Key key1own = world_P_body_key_pairs_[k].first; + const Key key1e = e->world_P_body_key_pairs()[k].first; + const Key key2own = world_P_body_key_pairs_[k].second; + const Key key2e = e->world_P_body_key_pairs()[k].second; + if ( !(key1own == key1e) || !(key2own == key2e) ){ + keyPairsEqual = false; break; + } + } + }else{ 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() - && gammas_ == e->getGammas() && body_P_sensors_ == e->body_P_sensors(); + && gammas_ == e->getGammas() && keyPairsEqual && extrinsicCalibrationEqual; } /** @@ -249,8 +277,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< * and the error vector b. Note that the jacobians are computed for a given point. */ void computeJacobiansWithTriangulatedPoint(FBlocks& Fs, Matrix& E, Vector& b, - const Values& values) const - override { + const Values& values) const { if (!this->result_) { throw("computeJacobiansWithTriangulatedPoint"); } else { // valid result: compute jacobians @@ -296,7 +323,7 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< /// linearize and return a Hessianfactor that is an approximation of error(p) boost::shared_ptr > createHessianFactor( const Values& values, const double lambda = 0.0, bool diagonalDamping = - false) const override { + false) const { // we may have multiple cameras sharing the same extrinsic cals, hence the number // of keys may be smaller than 2 * nrMeasurements (which is the upper bound where we @@ -313,29 +340,29 @@ class SmartProjectionPoseFactorRollingShutter : public SmartProjectionFactor< throw std::runtime_error("SmartProjectionPoseFactorRollingShutter: " "measured_.size() inconsistent with input"); - // triangulate 3D point at given linearization point - triangulateSafe(cameras(values)); - - if (!this->result_) { // failed: return "empty/zero" Hessian - for (Matrix& m : Gs) - m = Matrix::Zero(DimPose, DimPose); - for (Vector& v : gs) - v = Vector::Zero(DimPose); - return boost::make_shared < RegularHessianFactor - > ( this->keys_, Gs, gs, 0.0); - } - - // compute Jacobian given triangulated 3D Point - FBlocks Fs; - Matrix F, E; - Vector b; - computeJacobiansWithTriangulatedPoint(Fs, E, b, values); - - // Whiten using noise model - this->noiseModel_->WhitenSystem(E, b); - for (size_t i = 0; i < Fs.size(); i++) - Fs[i] = this->noiseModel_->Whiten(Fs[i]); - +// // triangulate 3D point at given linearization point +// triangulateSafe(cameras(values)); +// +// if (!this->result_) { // failed: return "empty/zero" Hessian +// for (Matrix& m : Gs) +// m = Matrix::Zero(DimPose, DimPose); +// for (Vector& v : gs) +// v = Vector::Zero(DimPose); +// return boost::make_shared < RegularHessianFactor +// > ( this->keys_, Gs, gs, 0.0); +// } +// +// // compute Jacobian given triangulated 3D Point +// FBlocks Fs; +// Matrix F, E; +// Vector b; +// computeJacobiansWithTriangulatedPoint(Fs, E, b, values); +// +// // Whiten using noise model +// this->noiseModel_->WhitenSystem(E, b); +// for (size_t i = 0; i < Fs.size(); i++) +// Fs[i] = this->noiseModel_->Whiten(Fs[i]); +// // // build augmented Hessian (with last row/column being the information vector) // Matrix3 P; // This::Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); diff --git a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp index d4c268b3c..fe662932c 100644 --- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp +++ b/gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp @@ -10,13 +10,13 @@ * -------------------------------------------------------------------------- */ /** - * @file testSmartProjectionPoseFactorRollingShutter.cpp - * @brief Unit tests for SmartProjectionPoseFactorRollingShutter Class + * @file testSmartProjectionPoseFactorRollingShutterRollingShutter.cpp + * @brief Unit tests for SmartProjectionPoseFactorRollingShutterRollingShutter Class * @author Luca Carlone * @date July 2021 */ -#include "gtsam/slam/tests/smartFactorScenarios.h" +//#include "gtsam/slam/tests/smartFactorScenarios.h" #include #include #include @@ -27,6 +27,7 @@ #include #include +using namespace gtsam; using namespace boost::assign; using namespace std::placeholders; @@ -47,17 +48,15 @@ static Symbol x3('X', 3); static Point2 measurement1(323.0, 240.0); LevenbergMarquardtParams lmParams; -// Make more verbose like so (in tests): -// params.verbosityLM = LevenbergMarquardtParams::SUMMARY; +typedef SmartProjectionPoseFactorRollingShutter SmartFactorRS; -/* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor) { - using namespace vanillaPose; - SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); +/* ************************************************************************* */ +TEST( SmartProjectionPoseFactorRollingShutter, Constructor) { + SmartFactorRS::shared_ptr factor1(new SmartFactorRS(model)); } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor2) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor2) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -65,14 +64,14 @@ TEST( SmartProjectionPoseFactor, Constructor2) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor3) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor3) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Constructor4) { +TEST( SmartProjectionPoseFactorRollingShutter, Constructor4) { using namespace vanillaPose; SmartProjectionParams params; params.setRankTolerance(rankTol); @@ -81,7 +80,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, params) { +TEST( SmartProjectionPoseFactorRollingShutter, params) { using namespace vanillaPose; SmartProjectionParams params; double rt = params.getRetriangulationThreshold(); @@ -92,7 +91,7 @@ TEST( SmartProjectionPoseFactor, params) { } /* ************************************************************************* * -TEST( SmartProjectionPoseFactor, Equals ) { +TEST( SmartProjectionPoseFactorRollingShutter, Equals ) { using namespace vanillaPose; SmartFactor::shared_ptr factor1(new SmartFactor(model, sharedK)); factor1->add(measurement1, x1); @@ -104,7 +103,7 @@ TEST( SmartProjectionPoseFactor, Equals ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, noiseless ) { +TEST( SmartProjectionPoseFactorRollingShutter, noiseless ) { using namespace vanillaPose; @@ -162,7 +161,7 @@ TEST( SmartProjectionPoseFactor, noiseless ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, noisy ) { +TEST( SmartProjectionPoseFactorRollingShutter, noisy ) { using namespace vanillaPose; @@ -196,7 +195,7 @@ TEST( SmartProjectionPoseFactor, noisy ) { } /* ************************************************************************* -TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { +TEST(SmartProjectionPoseFactorRollingShutter, smartFactorWithSensorBodyTransform) { using namespace vanillaPose; // create arbitrary body_T_sensor (transforms from sensor to body) @@ -271,7 +270,7 @@ TEST(SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_smart_projection_factor ) { using namespace vanillaPose2; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -332,7 +331,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, Factors ) { +TEST( SmartProjectionPoseFactorRollingShutter, Factors ) { using namespace vanillaPose; @@ -496,7 +495,7 @@ TEST( SmartProjectionPoseFactor, Factors ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_iterative_smart_projection_factor ) { using namespace vanillaPose; @@ -550,7 +549,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, jacobianSVD ) { +TEST( SmartProjectionPoseFactorRollingShutter, jacobianSVD ) { using namespace vanillaPose; @@ -606,7 +605,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, landmarkDistance ) { +TEST( SmartProjectionPoseFactorRollingShutter, landmarkDistance ) { using namespace vanillaPose; @@ -665,7 +664,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { +TEST( SmartProjectionPoseFactorRollingShutter, dynamicOutlierRejection ) { using namespace vanillaPose; @@ -731,7 +730,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { +TEST( SmartProjectionPoseFactorRollingShutter, 3poses_projection_factor ) { using namespace vanillaPose2; @@ -778,7 +777,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, CheckHessian) { +TEST( SmartProjectionPoseFactorRollingShutter, CheckHessian) { KeyVector views {x1, x2, x3}; @@ -786,7 +785,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { // Two slightly different cameras Pose3 pose2 = level_pose - * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); + * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Pose3 pose3 = pose2 * Pose3(Rot3::RzRyRx(-0.05, 0.0, -0.05), Point3(0, 0, 0)); Camera cam2(pose2, sharedK); Camera cam3(pose3, sharedK); @@ -860,7 +859,7 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { } /* ************************************************************************* -TEST( SmartProjectionPoseFactor, Hessian ) { +TEST( SmartProjectionPoseFactorRollingShutter, Hessian ) { using namespace vanillaPose2;