diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index 10de39049..56c214264 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -76,10 +76,6 @@ protected: typedef Eigen::Matrix VectorD; typedef Eigen::Matrix Matrix2; - /// An optional sensor pose, in the body frame (one for all cameras) - /// This seems to make sense for a CameraTrack, not for a CameraSet - boost::optional body_P_sensor_; - // check that noise model is isotropic and the same // TODO, this is hacky, we should just do this via constructor, not add void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) { @@ -107,17 +103,10 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; + /// We use the new CameraSte data structure to refer to a set of cameras typedef CameraSet Cameras; - /** - * Constructor - * @param body_P_sensor is the transform from sensor to body frame (default identity) - */ - SmartFactorBase(boost::optional body_P_sensor = boost::none) : - body_P_sensor_(body_P_sensor) { - } - - /** Virtual destructor */ + /// Virtual destructor, subclasses from NonlinearFactor virtual ~SmartFactorBase() { } @@ -193,8 +182,6 @@ public: std::cout << "measurement, p = " << measured_[k] << "\t"; noiseModel_->print("noise model = "); } - if (this->body_P_sensor_) - this->body_P_sensor_->print(" sensor pose in body frame: "); Base::print("", keyFormatter); } @@ -208,10 +195,7 @@ public: areMeasurementsEqual = false; break; } - return e && Base::equals(p, tol) && areMeasurementsEqual - && ((!body_P_sensor_ && !e->body_P_sensor_) - || (body_P_sensor_ && e->body_P_sensor_ - && body_P_sensor_->equals(*e->body_P_sensor_))); + return e && Base::equals(p, tol) && areMeasurementsEqual; } /// Compute reprojection errors @@ -221,27 +205,10 @@ public: /** * Compute reprojection errors and derivatives - * TODO: the treatment of body_P_sensor_ is weird: the transformation - * is applied in the caller, but the derivatives are computed here. */ Vector reprojectionError(const Cameras& cameras, const Point3& point, typename Cameras::FBlocks& F, Matrix& E) const { - - Vector b = cameras.reprojectionError(point, measured_, F, E); - - // Apply sensor chain rule if needed TODO: no simpler way ?? - if (body_P_sensor_) { - size_t m = keys_.size(); - for (size_t i = 0; i < m; i++) { - const Pose3& pose_i = cameras[i].pose(); - Pose3 w_Pose_body = pose_i.compose(body_P_sensor_->inverse()); - Matrix66 J; - Pose3 world_P_body = w_Pose_body.compose(*body_P_sensor_, J); - F[i].leftCols(6) *= J; - } - } - - return b; + return cameras.reprojectionError(point, measured_, F, E); } /// Calculate vector of re-projection errors, noise model applied @@ -582,7 +549,6 @@ private: void serialize(ARCHIVE & ar, const unsigned int version) { ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar & BOOST_SERIALIZATION_NVP(measured_); - ar & BOOST_SERIALIZATION_NVP(body_P_sensor_); } }; // end class SmartFactorBase diff --git a/gtsam/slam/SmartProjectionCameraFactor.h b/gtsam/slam/SmartProjectionCameraFactor.h index 8381c847e..7ca8a4e1d 100644 --- a/gtsam/slam/SmartProjectionCameraFactor.h +++ b/gtsam/slam/SmartProjectionCameraFactor.h @@ -56,17 +56,15 @@ public: * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations * @param isImplicit if set to true linearize the factor to an implicit Schur factor - * @param body_P_sensor is the transform from body to sensor frame (default identity) */ SmartProjectionCameraFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, const bool isImplicit = false, - boost::optional body_P_sensor = boost::none) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor), isImplicit_( + const bool enableEPI = false, const bool isImplicit = false) : + Base(rankTol, linThreshold, manageDegeneracy, enableEPI), isImplicit_( isImplicit) { if (linThreshold != -1) { - std::cout << "SmartProjectionCameraFactor: linThreshold " - << linThreshold << std::endl; + std::cout << "SmartProjectionCameraFactor: linThreshold " << linThreshold + << std::endl; } } diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index b4fad72fb..f99ce7085 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -116,16 +116,14 @@ public: * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from sensor to body frame (default identity) */ SmartProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) : - Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( - 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( + rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_( + manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( @@ -336,8 +334,8 @@ public: m = zeros(Base::Dim, Base::Dim); BOOST_FOREACH(Vector& v, gs) v = zero(Base::Dim); - return boost::make_shared >(this->keys_, Gs, gs, - 0.0); + return boost::make_shared >(this->keys_, + Gs, gs, 0.0); } // instead, if we want to manage the exception.. @@ -372,7 +370,7 @@ public: { std::vector Fblocks; f = computeJacobiansWithTriangulatedPoint(Fblocks, E, b, cameras); - Base::FillDiagonalF(Fblocks,F); // expensive ! + Base::FillDiagonalF(Fblocks, F); // expensive ! } // Schur complement trick @@ -390,7 +388,8 @@ public: // Note the minus sign above! g has negative b. // Informal reasoning: when we write the error as 0.5*|Ax-b|^2 // the derivative is A'*(Ax-b), and at x=0, this becomes -A'*b - gs_vector.noalias() = - F.transpose() * (b - (E * (P * (E.transpose() * b)))); + gs_vector.noalias() = -F.transpose() + * (b - (E * (P * (E.transpose() * b)))); // Populate Gs and gs int GsCount2 = 0; @@ -410,7 +409,8 @@ public: this->state_->gs = gs; this->state_->f = f; } - return boost::make_shared >(this->keys_, Gs, gs, f); + return boost::make_shared >(this->keys_, Gs, + gs, f); } // create factor @@ -541,8 +541,9 @@ public: } /// Version that takes values, and creates the point - bool triangulateAndComputeJacobians(std::vector& Fblocks, - Matrix& E, Vector& b, const Values& values) const { + bool triangulateAndComputeJacobians( + std::vector& Fblocks, Matrix& E, Vector& b, + const Values& values) const { Cameras cameras; bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) @@ -566,7 +567,7 @@ public: Cameras cameras; bool nonDegenerate = computeCamerasAndTriangulate(values, cameras); if (nonDegenerate) - return Base::reprojectionError(cameras, point_); + return Base::reprojectionError(cameras, point_); else return zero(cameras.size() * 2); } @@ -613,7 +614,7 @@ public: // 3D parameterization of point at infinity const Point2& zi = this->measured_.at(0); this->point_ = cameras.front().backprojectPointAtInfinity(zi); - return Base::totalReprojectionErrorAtInfinity(cameras,this->point_); + return Base::totalReprojectionErrorAtInfinity(cameras, this->point_); } else { // Just use version in base class return Base::totalReprojectionError(cameras, point_); diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index 5cfd74913..2ee807d9d 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -64,15 +64,16 @@ public: * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from sensor to body frame (default identity) */ SmartProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN, + double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_( + linearizeTo) { + } /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} @@ -150,12 +151,9 @@ public: */ typename Base::Cameras cameras(const Values& values) const { typename Base::Cameras cameras; - size_t i=0; + size_t i = 0; BOOST_FOREACH(const Key& k, this->keys_) { Pose3 pose = values.at(k); - if(Base::body_P_sensor_) - pose = pose.compose(*(Base::body_P_sensor_)); - Camera camera(pose, sharedKs_[i++]); cameras.push_back(camera); } diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index a106bf382..ba7b7bf51 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -41,8 +41,6 @@ Symbol l1('l', 1), l2('l', 2), l3('l', 3); Key c1 = 1, c2 = 2, c3 = 3; static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); typedef SmartProjectionCameraFactor SmartFactor; typedef SmartProjectionCameraFactor SmartFactorBundler; @@ -100,17 +98,6 @@ TEST( SmartProjectionCameraFactor, Constructor4) { factor1.add(measurement1, x1, unit2); } -/* ************************************************************************* */ -TEST( SmartProjectionCameraFactor, ConstructorWithTransform) { - using namespace vanilla; - bool manageDegeneracy = true; - bool isImplicit = false; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, isImplicit, - enableEPI, body_P_sensor1); - factor1.add(measurement1, x1, unit2); -} - /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Equals ) { using namespace vanilla; diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index caa0e5162..9345b5f45 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -50,9 +50,6 @@ static Symbol x2('X', 2); static Symbol x3('X', 3); static Point2 measurement1(323.0, 240.0); -static Pose3 body_P_sensor1(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), - Point3(0.25, -0.10, 1.0)); - typedef SmartProjectionPoseFactor SmartFactor; typedef SmartProjectionPoseFactor SmartFactorBundler; @@ -80,16 +77,6 @@ TEST( SmartProjectionPoseFactor, Constructor4) { factor1.add(measurement1, x1, model, sharedK); } -/* ************************************************************************* */ -TEST( SmartProjectionPoseFactor, ConstructorWithTransform) { - using namespace vanillaPose; - bool manageDegeneracy = true; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, - body_P_sensor1); - factor1.add(measurement1, x1, model, sharedK); -} - /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Equals ) { using namespace vanillaPose; @@ -277,117 +264,6 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { tictoc_print_(); } -/* *************************************************************************/ -TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ) { - - using namespace vanillaPose; - - // create arbitrary body_Pose_sensor (transforms from sensor to body) - Pose3 sensor_to_body = Pose3(Rot3::ypr(-M_PI / 2, 0., -M_PI / 2), - Point3(1, 1, 1)); // Pose3(); // - - // These are the poses we want to estimate, from camera measurements - Pose3 bodyPose1 = level_pose.compose(sensor_to_body.inverse()); - Pose3 bodyPose2 = pose_right.compose(sensor_to_body.inverse()); - Pose3 bodyPose3 = pose_above.compose(sensor_to_body.inverse()); - - vector measurements_cam1, measurements_cam2, measurements_cam3; - - // Project three landmarks into three cameras - projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); - projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - - // Create smart factors - vector views; - views.push_back(x1); - views.push_back(x2); - views.push_back(x3); - - double rankTol = 1; - double linThreshold = -1; - bool manageDegeneracy = false; - bool enableEPI = false; - - SmartFactor::shared_ptr smartFactor1( - new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, - sensor_to_body)); - smartFactor1->add(measurements_cam1, views, model, sharedK); - - SmartFactor::shared_ptr smartFactor2( - new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, - sensor_to_body)); - smartFactor2->add(measurements_cam2, views, model, sharedK); - - SmartFactor::shared_ptr smartFactor3( - new SmartFactor(rankTol, linThreshold, manageDegeneracy, enableEPI, - sensor_to_body)); - smartFactor3->add(measurements_cam3, views, model, sharedK); - - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - // Put all factors in factor graph, adding priors - NonlinearFactorGraph graph; - graph.push_back(smartFactor1); - graph.push_back(smartFactor2); - graph.push_back(smartFactor3); - graph.push_back(PriorFactor(x1, bodyPose1, noisePrior)); - graph.push_back(PriorFactor(x2, bodyPose2, noisePrior)); - - // Check errors at ground truth poses - Values gtValues; - gtValues.insert(x1, bodyPose1); - gtValues.insert(x2, bodyPose2); - gtValues.insert(x3, bodyPose3); - double actualError = graph.error(gtValues); - double expectedError = 0.0; - DOUBLES_EQUAL(expectedError, actualError, 1e-7) - - Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI / 100, 0., -M_PI / 100), - Point3(0.1, 0.1, 0.1)); - Values values; - values.insert(x1, bodyPose1); - values.insert(x2, bodyPose2); - // initialize third pose with some noise, we expect it to move back to original pose_above - values.insert(x3, bodyPose3 * noise_pose); - - LevenbergMarquardtParams params; - Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); - result = optimizer.optimize(); - - // result.print("results of 3 camera, 3 landmark optimization \n"); - if (isDebugTest) - result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(bodyPose3, result.at(x3))); - - // Check derivatives - - // Calculate E and P using computeEP, triangulates - Matrix actualE; - smartFactor1->triangulateAndComputeE(actualE, values); - - // Calculate expected derivative for point (easiest to check) - SmartFactor::Cameras cameras = smartFactor1->cameras(values); - boost::function f = // - boost::bind(&SmartFactor::reprojectionError, *smartFactor1, cameras, _1); - boost::optional point = smartFactor1->point(); - CHECK(point); - - // TODO, this is really a test of CameraSet - Matrix expectedE = numericalDerivative11(f, *point); - EXPECT(assert_equal(expectedE, actualE, 1e-7)); - - // Calculate using reprojectionError - Matrix E; - SmartFactor::Cameras::FBlocks F; - Vector actualErrors = smartFactor1->reprojectionError(cameras, *point, F, E); - EXPECT(assert_equal(expectedE, E, 1e-7)); - - // Success ! The derivatives of reprojectionError now agree with f ! - EXPECT(assert_equal(f(*point), actualErrors, 1e-7)); -} - /* *************************************************************************/ TEST( SmartProjectionPoseFactor, Factors ) { @@ -599,15 +475,15 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -654,17 +530,17 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, sharedK); @@ -720,22 +596,22 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, sharedK); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, sharedK); @@ -782,15 +658,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + new SmartFactor(1, -1, false, false, JACOBIAN_Q)); smartFactor1->add(measurements_cam1, views, model, sharedK); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + new SmartFactor(1, -1, false, false, JACOBIAN_Q)); smartFactor2->add(measurements_cam2, views, model, sharedK); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); + new SmartFactor(1, -1, false, false, JACOBIAN_Q)); smartFactor3->add(measurements_cam3, views, model, sharedK); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 1cb026040..68b396cd6 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -130,16 +130,15 @@ public: */ SmartStereoProjectionFactor(const double rankTol, const double linThreshold, const bool manageDegeneracy, const bool enableEPI, - boost::optional body_P_sensor = boost::none, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - SmartFactorStatePtr state = SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : - Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( - 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( + double dynamicOutlierRejectionThreshold = -1, SmartFactorStatePtr state = + SmartFactorStatePtr(new SmartStereoProjectionFactorState())) : + rankTolerance_(rankTol), retriangulationThreshold_(1e-5), manageDegeneracy_( + manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( - false), verboseCheirality_(false), state_(state), - landmarkDistanceThreshold_(landmarkDistanceThreshold), - dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) { + false), verboseCheirality_(false), state_(state), landmarkDistanceThreshold_( + landmarkDistanceThreshold), dynamicOutlierRejectionThreshold_( + dynamicOutlierRejectionThreshold) { } /** Virtual destructor */ diff --git a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h index 3e0c6476f..2f4677835 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionPoseFactor.h @@ -65,15 +65,16 @@ public: * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, * otherwise the factor is simply neglected * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from body to sensor frame (default identity) */ SmartStereoProjectionPoseFactor(const double rankTol = 1, const double linThreshold = -1, const bool manageDegeneracy = false, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, + const bool enableEPI = false, LinearizationMode linearizeTo = HESSIAN, + double landmarkDistanceThreshold = 1e10, double dynamicOutlierRejectionThreshold = -1) : - Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor, - landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {} + Base(rankTol, linThreshold, manageDegeneracy, enableEPI, + landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_( + linearizeTo) { + } /** Virtual destructor */ virtual ~SmartStereoProjectionPoseFactor() {} diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp index 4cc8e66ff..48dfa1ff0 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionPoseFactor.cpp @@ -106,15 +106,6 @@ TEST( SmartStereoProjectionPoseFactor, Constructor4) { factor1.add(measurement1, poseKey1, model, K); } -/* ************************************************************************* */ -TEST( SmartStereoProjectionPoseFactor, ConstructorWithTransform) { - bool manageDegeneracy = true; - bool enableEPI = false; - SmartFactor factor1(rankTol, linThreshold, manageDegeneracy, enableEPI, - body_P_sensor1); - factor1.add(measurement1, poseKey1, model, K); -} - /* ************************************************************************* */ TEST( SmartStereoProjectionPoseFactor, Equals ) { SmartFactor::shared_ptr factor1(new SmartFactor()); @@ -345,15 +336,15 @@ TEST( SmartStereoProjectionPoseFactor, jacobianSVD ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD)); + new SmartFactor(1, -1, false, false, JACOBIAN_SVD)); smartFactor3->add(measurements_cam3, views, model, K); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -414,17 +405,17 @@ TEST( SmartStereoProjectionPoseFactor, landmarkDistance ) { cam2, cam3, landmark3); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist)); smartFactor3->add(measurements_cam3, views, model, K); @@ -493,22 +484,22 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { measurements_cam4.at(0) = measurements_cam4.at(0) + StereoPoint2(10, 10, 1); // add outlier SmartFactor::shared_ptr smartFactor1( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor1->add(measurements_cam1, views, model, K); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor2->add(measurements_cam2, views, model, K); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor3->add(measurements_cam3, views, model, K); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, + new SmartFactor(1, -1, false, false, JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); smartFactor4->add(measurements_cam4, views, model, K); @@ -567,13 +558,13 @@ TEST( SmartStereoProjectionPoseFactor, dynamicOutlierRejection ) { // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); // stereo_projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); // -// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor1->add(measurements_cam1, views, model, K); // -// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor2->add(measurements_cam2, views, model, K); // -// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q)); +// SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, JACOBIAN_Q)); // smartFactor3->add(measurements_cam3, views, model, K); // // const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);