using parameters in smart projection factors constructors.. breaking the API, but now is way more elegant
Luca
parent
f8205bfe02
commit
cd6c5ca0bd
|
|
@ -320,21 +320,21 @@ Point3 triangulatePoint3(
|
|||
|
||||
struct TriangulationParameters {
|
||||
|
||||
const double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
|
||||
const bool enableEPI; ///< if set to true, will refine triangulation using LM
|
||||
double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate
|
||||
bool enableEPI; ///< if set to true, will refine triangulation using LM
|
||||
|
||||
/**
|
||||
* if the landmark is triangulated at distance larger than this,
|
||||
* result is flagged as degenerate.
|
||||
*/
|
||||
const double landmarkDistanceThreshold; //
|
||||
double landmarkDistanceThreshold; //
|
||||
|
||||
/**
|
||||
* If this is nonnegative the we will check if the average reprojection error
|
||||
* is smaller than this threshold after triangulation, otherwise result is
|
||||
* flagged as degenerate.
|
||||
*/
|
||||
const double dynamicOutlierRejectionThreshold;
|
||||
double dynamicOutlierRejectionThreshold;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
|
|
|
|||
|
|
@ -48,12 +48,12 @@ class GTSAM_EXPORT SmartProjectionParams {
|
|||
|
||||
public:
|
||||
|
||||
const LinearizationMode linearizationMode; ///< How to linearize the factor
|
||||
const DegeneracyMode degeneracyMode; ///< How to linearize the factor
|
||||
LinearizationMode linearizationMode; ///< How to linearize the factor
|
||||
DegeneracyMode degeneracyMode; ///< How to linearize the factor
|
||||
|
||||
/// @name Parameters governing the triangulation
|
||||
/// @{
|
||||
const TriangulationParameters triangulationParameters;
|
||||
mutable TriangulationParameters triangulationParameters;
|
||||
const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate
|
||||
/// @}
|
||||
|
||||
|
|
@ -64,10 +64,10 @@ public:
|
|||
/// @}
|
||||
|
||||
// Constructor
|
||||
SmartProjectionParams(LinearizationMode linMode = HESSIAN,
|
||||
DegeneracyMode degMode = IGNORE_DEGENERACY, double rankTol = 1,
|
||||
bool enableEPI = false, double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1):
|
||||
SmartProjectionParams(const LinearizationMode linMode = HESSIAN,
|
||||
const DegeneracyMode degMode = IGNORE_DEGENERACY, const double rankTol = 1,
|
||||
const bool enableEPI = false, const double landmarkDistanceThreshold = 1e10,
|
||||
const double dynamicOutlierRejectionThreshold = -1):
|
||||
linearizationMode(linMode), degeneracyMode(degMode),
|
||||
triangulationParameters(rankTol, enableEPI,
|
||||
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold),
|
||||
|
|
@ -89,15 +89,36 @@ public:
|
|||
inline LinearizationMode getLinearizationMode() const {
|
||||
return linearizationMode;
|
||||
}
|
||||
/** return cheirality verbosity */
|
||||
inline DegeneracyMode getDegeneracyMode() const {
|
||||
return degeneracyMode;
|
||||
}
|
||||
inline TriangulationParameters getTriangulationParameters() const {
|
||||
return triangulationParameters;
|
||||
}
|
||||
inline bool getVerboseCheirality() const {
|
||||
return verboseCheirality;
|
||||
}
|
||||
/** return flag for throwing cheirality exceptions */
|
||||
inline bool getThrowCheirality() const {
|
||||
return throwCheirality;
|
||||
}
|
||||
|
||||
inline void setLinearizationMode(LinearizationMode linMode) {
|
||||
linearizationMode = linMode;
|
||||
}
|
||||
inline void setDegeneracyMode(DegeneracyMode degMode) {
|
||||
degeneracyMode = degMode;
|
||||
}
|
||||
inline void setRankTolerance(double rankTol) {
|
||||
triangulationParameters.rankTolerance = rankTol;
|
||||
}
|
||||
inline void setEnableEPI(bool enableEPI) {
|
||||
triangulationParameters.enableEPI = enableEPI;
|
||||
}
|
||||
inline void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) {
|
||||
triangulationParameters.landmarkDistanceThreshold = landmarkDistanceThreshold;
|
||||
}
|
||||
inline void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) {
|
||||
triangulationParameters.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
|
|
@ -134,36 +155,16 @@ public:
|
|||
/// shorthand for a set of cameras
|
||||
typedef CameraSet<CAMERA> Cameras;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param rankTol tolerance used to check if point triangulation is degenerate
|
||||
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
|
||||
* @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
|
||||
*/
|
||||
SmartProjectionFactor(LinearizationMode linearizationMode = HESSIAN,
|
||||
double rankTolerance = 1, DegeneracyMode degeneracyMode =
|
||||
IGNORE_DEGENERACY, bool enableEPI = false,
|
||||
double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1,
|
||||
boost::optional<Pose3> body_P_sensor = boost::none) :
|
||||
Base(body_P_sensor),
|
||||
params_(linearizationMode, degeneracyMode,
|
||||
rankTolerance, enableEPI, landmarkDistanceThreshold,
|
||||
dynamicOutlierRejectionThreshold), //
|
||||
result_(TriangulationResult::Degenerate()) {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param body_P_sensor pose of the camera in the body frame
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
// SmartProjectionFactor(const boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
// const SmartProjectionParams params = SmartProjectionParams()) :
|
||||
// Base(body_P_sensor),
|
||||
// params_(params), //
|
||||
// result_(TriangulationResult::Degenerate()) {}
|
||||
SmartProjectionFactor(const boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
const SmartProjectionParams params = SmartProjectionParams()) :
|
||||
Base(body_P_sensor),
|
||||
params_(params), //
|
||||
result_(TriangulationResult::Degenerate()) {}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionFactor() {
|
||||
|
|
|
|||
|
|
@ -55,33 +55,16 @@ public:
|
|||
/// shorthand for a smart pointer to a factor
|
||||
typedef boost::shared_ptr<This> shared_ptr;
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param rankTol tolerance used to check if point triangulation is degenerate
|
||||
* @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization)
|
||||
* @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint,
|
||||
* otherwise the factor is simply neglected (this functionality is deprecated)
|
||||
* @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 boost::shared_ptr<CALIBRATION> K, const double rankTol = 1,
|
||||
const double linThreshold = -1, const DegeneracyMode manageDegeneracy = IGNORE_DEGENERACY,
|
||||
const bool enableEPI = false, boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
|
||||
double dynamicOutlierRejectionThreshold = -1) :
|
||||
Base(linearizeTo, rankTol, manageDegeneracy, enableEPI, landmarkDistanceThreshold,
|
||||
dynamicOutlierRejectionThreshold, body_P_sensor), K_(K) {}
|
||||
|
||||
/**
|
||||
* Constructor
|
||||
* @param K (fixed) calibration, assumed to be the same for all cameras
|
||||
* @param body_P_sensor pose of the camera in the body frame
|
||||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
// SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
|
||||
// const boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
// const SmartProjectionParams params = SmartProjectionParams()) :
|
||||
// Base(body_P_sensor, params), K_(K) {}
|
||||
SmartProjectionPoseFactor(const boost::shared_ptr<CALIBRATION> K,
|
||||
const boost::optional<Pose3> body_P_sensor = boost::none,
|
||||
const SmartProjectionParams params = SmartProjectionParams()) :
|
||||
Base(body_P_sensor, params), K_(K) {}
|
||||
|
||||
/** Virtual destructor */
|
||||
virtual ~SmartProjectionPoseFactor() {}
|
||||
|
|
|
|||
|
|
@ -61,6 +61,7 @@ Camera cam1(level_pose, K2);
|
|||
Camera cam2(pose_right, K2);
|
||||
Camera cam3(pose_above, K2);
|
||||
typedef GeneralSFMFactor<Camera, Point3> SFMFactor;
|
||||
SmartProjectionParams params;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -30,8 +30,6 @@ using namespace boost::assign;
|
|||
|
||||
static bool isDebugTest = false;
|
||||
|
||||
static double rankTol = 1.0;
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
|
|
@ -42,6 +40,8 @@ Key c1 = 1, c2 = 2, c3 = 3;
|
|||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
|
||||
static double rankTol = 1.0;
|
||||
|
||||
template<class CALIBRATION>
|
||||
PinholeCamera<CALIBRATION> perturbCameraPoseAndCalibration(
|
||||
const PinholeCamera<CALIBRATION>& camera) {
|
||||
|
|
@ -78,7 +78,8 @@ TEST( SmartProjectionCameraFactor, Constructor) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor2) {
|
||||
using namespace vanilla;
|
||||
SmartFactor factor1(gtsam::HESSIAN, rankTol);
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(boost::none, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -91,7 +92,8 @@ TEST( SmartProjectionCameraFactor, Constructor3) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionCameraFactor, Constructor4) {
|
||||
using namespace vanilla;
|
||||
SmartFactor factor1(gtsam::HESSIAN, rankTol);
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(boost::none, params);
|
||||
factor1.add(measurement1, x1, unit2);
|
||||
}
|
||||
|
||||
|
|
@ -257,12 +259,12 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) {
|
|||
EXPECT(assert_equal(expected, actual, 1));
|
||||
|
||||
// Optimize
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams lmParams;
|
||||
if (isDebugTest) {
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
}
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7));
|
||||
|
|
@ -338,14 +340,14 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) {
|
|||
if (isDebugTest)
|
||||
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams lmParams;
|
||||
if (isDebugTest)
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if (isDebugTest)
|
||||
params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
|
||||
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
|
||||
|
|
@ -415,17 +417,17 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) {
|
|||
if (isDebugTest)
|
||||
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
params.relativeErrorTol = 1e-8;
|
||||
params.absoluteErrorTol = 0;
|
||||
params.maxIterations = 20;
|
||||
LevenbergMarquardtParams lmParams;
|
||||
lmParams.relativeErrorTol = 1e-8;
|
||||
lmParams.absoluteErrorTol = 0;
|
||||
lmParams.maxIterations = 20;
|
||||
if (isDebugTest)
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if (isDebugTest)
|
||||
params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
|
||||
// GaussianFactorGraph::shared_ptr GFG = graph.linearize(values);
|
||||
|
|
@ -494,17 +496,17 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) {
|
|||
if (isDebugTest)
|
||||
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
params.relativeErrorTol = 1e-8;
|
||||
params.absoluteErrorTol = 0;
|
||||
params.maxIterations = 20;
|
||||
LevenbergMarquardtParams lmParams;
|
||||
lmParams.relativeErrorTol = 1e-8;
|
||||
lmParams.absoluteErrorTol = 0;
|
||||
lmParams.maxIterations = 20;
|
||||
if (isDebugTest)
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if (isDebugTest)
|
||||
params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
|
||||
if (isDebugTest)
|
||||
|
|
@ -570,17 +572,17 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) {
|
|||
if (isDebugTest)
|
||||
values.at<Camera>(c3).print("Smart: Pose3 before optimization: ");
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
params.relativeErrorTol = 1e-8;
|
||||
params.absoluteErrorTol = 0;
|
||||
params.maxIterations = 20;
|
||||
LevenbergMarquardtParams lmParams;
|
||||
lmParams.relativeErrorTol = 1e-8;
|
||||
lmParams.absoluteErrorTol = 0;
|
||||
lmParams.maxIterations = 20;
|
||||
if (isDebugTest)
|
||||
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
|
||||
if (isDebugTest)
|
||||
params.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
lmParams.verbosity = NonlinearOptimizerParams::ERROR;
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
|
||||
if (isDebugTest)
|
||||
|
|
@ -805,9 +807,14 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
bool useEPI = false;
|
||||
|
||||
// Hessian version
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
params.setLinearizationMode(gtsam::HESSIAN);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setEnableEPI(useEPI);
|
||||
|
||||
SmartFactor::shared_ptr explicitFactor(
|
||||
new SmartFactor(gtsam::HESSIAN, rankTol,
|
||||
gtsam::IGNORE_DEGENERACY, useEPI));
|
||||
new SmartFactor(boost::none, params));
|
||||
explicitFactor->add(level_uv, c1, unit2);
|
||||
explicitFactor->add(level_uv_right, c2, unit2);
|
||||
|
||||
|
|
@ -817,9 +824,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) {
|
|||
dynamic_cast<HessianFactor&>(*gaussianHessianFactor);
|
||||
|
||||
// Implicit Schur version
|
||||
params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
|
||||
SmartFactor::shared_ptr implicitFactor(
|
||||
new SmartFactor(gtsam::IMPLICIT_SCHUR, rankTol,
|
||||
gtsam::IGNORE_DEGENERACY, useEPI));
|
||||
new SmartFactor(boost::none, params));
|
||||
implicitFactor->add(level_uv, c1, unit2);
|
||||
implicitFactor->add(level_uv_right, c2, unit2);
|
||||
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
|
||||
|
|
|
|||
|
|
@ -46,7 +46,7 @@ static Symbol x3('X', 3);
|
|||
|
||||
static Point2 measurement1(323.0, 240.0);
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams lmParams;
|
||||
// Make more verbose like so (in tests):
|
||||
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
|
||||
|
||||
|
|
@ -59,7 +59,9 @@ TEST( SmartProjectionPoseFactor, Constructor) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor2) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor factor1(sharedK, rankTol);
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(sharedK, boost::none, params);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -72,7 +74,9 @@ TEST( SmartProjectionPoseFactor, Constructor3) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, Constructor4) {
|
||||
using namespace vanillaPose;
|
||||
SmartFactor factor1(sharedK, rankTol);
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(rankTol);
|
||||
SmartFactor factor1(sharedK, boost::none, params);
|
||||
factor1.add(measurement1, x1, model);
|
||||
}
|
||||
|
||||
|
|
@ -229,15 +233,18 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
|
|||
views.push_back(x2);
|
||||
views.push_back(x3);
|
||||
|
||||
bool enableEPI = false;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(1.0);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setEnableEPI(false);
|
||||
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body);
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor1(K, sensor_to_body, params);
|
||||
smartFactor1.add(measurements_cam1, views, model);
|
||||
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body);
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor2(K, sensor_to_body, params);
|
||||
smartFactor2.add(measurements_cam2, views, model);
|
||||
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body);
|
||||
SmartProjectionPoseFactor<Cal3_S2> smartFactor3(K, sensor_to_body, params);
|
||||
smartFactor3.add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -266,9 +273,9 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){
|
|||
// initialize third pose with some noise, we expect it to move back to original pose3
|
||||
values.insert(x3, bodyPose3*noise_pose);
|
||||
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams lmParams;
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(bodyPose3,result.at<Pose3>(x3)));
|
||||
}
|
||||
|
|
@ -329,7 +336,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) {
|
|||
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8));
|
||||
}
|
||||
|
|
@ -552,7 +559,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) {
|
|||
values.at<Pose3>(x3)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-7));
|
||||
}
|
||||
|
|
@ -574,16 +581,23 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(1.0);
|
||||
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setEnableEPI(false);
|
||||
SmartFactor factor1(sharedK, boost::none, params);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -604,7 +618,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
|
|||
values.insert(x3, pose_above * noise_pose);
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8));
|
||||
}
|
||||
|
|
@ -628,19 +642,23 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(1.0);
|
||||
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
||||
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
|
||||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setEnableEPI(false);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -662,7 +680,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
|
|||
|
||||
// All factors are disabled and pose should remain where it is
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(values.at<Pose3>(x3), result.at<Pose3>(x3)));
|
||||
}
|
||||
|
|
@ -693,24 +711,25 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
|
||||
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
|
||||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor4(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(),
|
||||
gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor4->add(measurements_cam4, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -730,7 +749,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
|
|||
|
||||
// All factors are disabled and pose should remain where it is
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3)));
|
||||
}
|
||||
|
|
@ -752,19 +771,19 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setLinearizationMode(gtsam::JACOBIAN_Q);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY,
|
||||
false, Pose3(), gtsam::JACOBIAN_Q));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY,
|
||||
false, Pose3(), gtsam::JACOBIAN_Q));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY,
|
||||
false, Pose3(), gtsam::JACOBIAN_Q));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -784,7 +803,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
|
|||
values.insert(x3, pose_above * noise_pose);
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose_above, result.at<Pose3>(x3), 1e-8));
|
||||
}
|
||||
|
|
@ -840,7 +859,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) {
|
|||
|
||||
DOUBLES_EQUAL(48406055, graph.error(values), 1);
|
||||
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
Values result = optimizer.optimize();
|
||||
|
||||
DOUBLES_EQUAL(0, graph.error(result), 1e-9);
|
||||
|
|
@ -872,18 +891,19 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
double rankTol = 10;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(10);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, rankTol)); // HESSIAN, by default
|
||||
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, rankTol)); // HESSIAN, by default
|
||||
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, rankTol)); // HESSIAN, by default
|
||||
new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
NonlinearFactorGraph graph;
|
||||
|
|
@ -953,13 +973,16 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
|
||||
double rankTol = 50;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(50);
|
||||
params.setDegeneracyMode(gtsam::HANDLE_INFINITY);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK2, rankTol, -1.0, gtsam::HANDLE_INFINITY));
|
||||
new SmartFactor(sharedK2, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK2, rankTol, -1.0, gtsam::HANDLE_INFINITY));
|
||||
new SmartFactor(sharedK2, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -983,7 +1006,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
|
|||
values.insert(x3, pose3 * noise_pose);
|
||||
|
||||
// params.verbosityLM = LevenbergMarquardtParams::SUMMARY;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
Values result = optimizer.optimize();
|
||||
EXPECT(assert_equal(pose3, result.at<Pose3>(x3)));
|
||||
}
|
||||
|
|
@ -1012,18 +1035,20 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
double rankTol = 10;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(10);
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY));
|
||||
new SmartFactor(sharedK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -1058,7 +1083,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
|
|||
values.at<Pose3>(x3)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
|
||||
// Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY)
|
||||
|
|
@ -1212,7 +1237,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) {
|
|||
/* ************************************************************************* */
|
||||
TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
|
||||
using namespace bundlerPose;
|
||||
SmartFactor factor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY, false, Pose3(), gtsam::HESSIAN);
|
||||
SmartProjectionParams params;
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
SmartFactor factor(sharedBundlerK, boost::none, params);
|
||||
factor.add(measurement1, x1, model);
|
||||
}
|
||||
|
||||
|
|
@ -1270,7 +1297,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) {
|
|||
Point3(0.1, -0.1, 1.9)), values.at<Pose3>(x3)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
EXPECT(assert_equal(cam3.pose(), result.at<Pose3>(x3), 1e-6));
|
||||
}
|
||||
|
|
@ -1302,21 +1329,20 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
|
|||
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
|
||||
|
||||
double rankTol = 10;
|
||||
SmartProjectionParams params;
|
||||
params.setRankTolerance(10);
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor1(
|
||||
new SmartFactor(sharedBundlerK, rankTol, -1.0, gtsam::ZERO_ON_DEGENERACY,
|
||||
false, Pose3(), gtsam::HESSIAN));
|
||||
new SmartFactor(sharedBundlerK, boost::none, params));
|
||||
smartFactor1->add(measurements_cam1, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor2(
|
||||
new SmartFactor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY,
|
||||
false, Pose3(), gtsam::HESSIAN));
|
||||
new SmartFactor(sharedBundlerK, boost::none, params));
|
||||
smartFactor2->add(measurements_cam2, views, model);
|
||||
|
||||
SmartFactor::shared_ptr smartFactor3(
|
||||
new SmartFactor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY,
|
||||
false, Pose3(), gtsam::HESSIAN));
|
||||
new SmartFactor(sharedBundlerK, boost::none, params));
|
||||
smartFactor3->add(measurements_cam3, views, model);
|
||||
|
||||
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
|
||||
|
|
@ -1352,7 +1378,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
|
|||
values.at<Pose3>(x3)));
|
||||
|
||||
Values result;
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, params);
|
||||
LevenbergMarquardtOptimizer optimizer(graph, values, lmParams);
|
||||
result = optimizer.optimize();
|
||||
|
||||
EXPECT(
|
||||
|
|
|
|||
Loading…
Reference in New Issue