Skip to content

Commit

Permalink
deprecated SmartProjectionFactor constructor with offset
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert committed Aug 8, 2019
1 parent 0eef77f commit 20736b6
Show file tree
Hide file tree
Showing 5 changed files with 86 additions and 108 deletions.
3 changes: 3 additions & 0 deletions gtsam.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2499,6 +2499,9 @@ virtual class SmartProjectionPoseFactor: gtsam::NonlinearFactor {
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::SmartProjectionParams& params);
SmartProjectionPoseFactor(const gtsam::noiseModel::Base* noise,
const CALIBRATION* K,
const gtsam::Pose3& body_P_sensor,
Expand Down
37 changes: 28 additions & 9 deletions gtsam/slam/SmartProjectionFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,15 +79,15 @@ class SmartProjectionFactor: public SmartFactorBase<CAMERA> {

/**
* Constructor
* @param body_P_sensor pose of the camera in the body frame
* @param params internal parameters of the smart factors
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements
* @param params parameters for the smart projection factors
*/
SmartProjectionFactor(const SharedNoiseModel& sharedNoiseModel,
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(sharedNoiseModel, body_P_sensor), params_(params), //
result_(TriangulationResult::Degenerate()) {
}
SmartProjectionFactor(
const SharedNoiseModel& sharedNoiseModel,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel),
params_(params),
result_(TriangulationResult::Degenerate()) {}

/** Virtual destructor */
virtual ~SmartProjectionFactor() {
Expand Down Expand Up @@ -443,7 +443,26 @@ class SmartProjectionFactor: public SmartFactorBase<CAMERA> {
/** return the farPoint state */
bool isFarPoint() const { return result_.farPoint(); }

private:
#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4
/// @name Deprecated
/// @{
// It does not make sense to optimize for a camera where the pose would not be
// the actual pose of the camera. An unfortunate consequence of deprecating
// this constructor means that we cannot optimize for calibration when the
// camera is offset from the body pose. That would need a new factor with
// (body) pose and calibration as variables. However, that use case is
// unlikely: when a global offset is know, calibration is typically known.
SmartProjectionFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::optional<Pose3> body_P_sensor,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, body_P_sensor),
params_(params),
result_(TriangulationResult::Degenerate()) {}
/// @}
#endif

private:

/// Serialization function
friend class boost::serialization::access;
Expand Down
29 changes: 22 additions & 7 deletions gtsam/slam/SmartProjectionPoseFactor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,16 +66,31 @@ class SmartProjectionPoseFactor: public SmartProjectionFactor<

/**
* Constructor
* @param Isotropic measurement noise
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements
* @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
* @param params parameters for the smart projection factors
*/
SmartProjectionPoseFactor(const SharedNoiseModel& sharedNoiseModel,
SmartProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor = boost::none,
const SmartProjectionParams& params = SmartProjectionParams()) :
Base(sharedNoiseModel, body_P_sensor, params), K_(K) {
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), K_(K) {
}

/**
* Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature measurements
* @param K (fixed) calibration, assumed to be the same for all cameras
* @param body_P_sensor pose of the camera in the body frame (optional)
* @param params parameters for the smart projection factors
*/
SmartProjectionPoseFactor(
const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<CALIBRATION> K,
const boost::optional<Pose3> body_P_sensor,
const SmartProjectionParams& params = SmartProjectionParams())
: SmartProjectionPoseFactor(sharedNoiseModel, K, params) {
this->body_P_sensor_ = body_P_sensor;
}

/** Virtual destructor */
Expand Down
67 changes: 4 additions & 63 deletions gtsam/slam/tests/testSmartProjectionFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,7 +71,7 @@ TEST(SmartProjectionFactor, Constructor) {
TEST(SmartProjectionFactor, Constructor2) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, boost::none, params);
SmartFactor factor1(unit2, params);
}

/* ************************************************************************* */
Expand All @@ -85,7 +85,7 @@ TEST(SmartProjectionFactor, Constructor3) {
TEST(SmartProjectionFactor, Constructor4) {
using namespace vanilla;
params.setRankTolerance(rankTol);
SmartFactor factor1(unit2, boost::none, params);
SmartFactor factor1(unit2, params);
factor1.add(measurement1, c1);
}

Expand Down Expand Up @@ -777,7 +777,7 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) {
params.setEnableEPI(useEPI);

SmartFactor::shared_ptr explicitFactor(
new SmartFactor(unit2, boost::none, params));
new SmartFactor(unit2, params));
explicitFactor->add(level_uv, c1);
explicitFactor->add(level_uv_right, c2);

Expand All @@ -789,7 +789,7 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) {
// Implicit Schur version
params.setLinearizationMode(gtsam::IMPLICIT_SCHUR);
SmartFactor::shared_ptr implicitFactor(
new SmartFactor(unit2, boost::none, params));
new SmartFactor(unit2, params));
implicitFactor->add(level_uv, c1);
implicitFactor->add(level_uv_right, c2);
GaussianFactor::shared_ptr gaussianImplicitSchurFactor =
Expand All @@ -810,65 +810,6 @@ TEST(SmartProjectionFactor, implicitJacobianFactor ) {
EXPECT(assert_equal(yActual, yExpected, 1e-7));
}


/* *************************************************************************/
TEST(SmartProjectionFactor, smartFactorWithSensorBodyTransform) {
using namespace vanilla;

// create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor = Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));

// These are the poses we want to estimate, from camera measurements
const Pose3 sensor_T_body = body_T_sensor.inverse();
Pose3 wTb1 = cam1.pose() * sensor_T_body;
Pose3 wTb2 = cam2.pose() * sensor_T_body;
Pose3 wTb3 = cam3.pose() * sensor_T_body;

// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2), landmark2(5, -0.5, 1.2), landmark3(5, 0, 3.0);

Point2Vector 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
KeyVector views {1, 2, 3};

SmartProjectionParams params;
params.setRankTolerance(1.0);
params.setDegeneracyMode(IGNORE_DEGENERACY);
params.setEnableEPI(false);

SmartFactor smartFactor1(unit2, body_T_sensor, params);
smartFactor1.add(measurements_cam1, views);

SmartFactor smartFactor2(unit2, body_T_sensor, params);
smartFactor2.add(measurements_cam2, views);

SmartFactor smartFactor3(unit2, body_T_sensor, params);
smartFactor3.add(measurements_cam3, views);

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);

// Check errors at ground truth poses
Values gtValues;
gtValues.insert(1, cam1);
gtValues.insert(2, cam2);
gtValues.insert(3, cam3);
double actualError = graph.error(gtValues);
double expectedError = 0.0;
DOUBLES_EQUAL(expectedError, actualError, 1e-7);
}

/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
Expand Down
58 changes: 29 additions & 29 deletions gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ TEST( SmartProjectionPoseFactor, Constructor2) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor1(model, sharedK, boost::none, params);
SmartFactor factor1(model, sharedK, params);
}

/* ************************************************************************* */
Expand All @@ -77,7 +77,7 @@ TEST( SmartProjectionPoseFactor, Constructor4) {
using namespace vanillaPose;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor1(model, sharedK, boost::none, params);
SmartFactor factor1(model, sharedK, params);
factor1.add(measurement1, x1);
}

Expand Down Expand Up @@ -569,18 +569,18 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) {
params.setLinearizationMode(gtsam::JACOBIAN_SVD);
params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY);
params.setEnableEPI(false);
SmartFactor factor1(model, sharedK, boost::none, params);
SmartFactor factor1(model, sharedK, params);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor2->add(measurements_cam2, views);

SmartFactor::shared_ptr smartFactor3(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor3->add(measurements_cam3, views);

const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
Expand Down Expand Up @@ -630,15 +630,15 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) {
params.setEnableEPI(false);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor2->add(measurements_cam2, views);

SmartFactor::shared_ptr smartFactor3(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor3->add(measurements_cam3, views);

const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
Expand Down Expand Up @@ -694,19 +694,19 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) {
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor2->add(measurements_cam2, views);

SmartFactor::shared_ptr smartFactor3(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor3->add(measurements_cam3, views);

SmartFactor::shared_ptr smartFactor4(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor4->add(measurements_cam4, views);

const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
Expand Down Expand Up @@ -749,15 +749,15 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) {
params.setLinearizationMode(gtsam::JACOBIAN_Q);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor2->add(measurements_cam2, views);

SmartFactor::shared_ptr smartFactor3(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor3->add(measurements_cam3, views);

const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
Expand Down Expand Up @@ -854,15 +854,15 @@ TEST( SmartProjectionPoseFactor, CheckHessian) {
params.setRankTolerance(10);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
new SmartFactor(model, sharedK, params)); // HESSIAN, by default
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
new SmartFactor(model, sharedK, params)); // HESSIAN, by default
smartFactor2->add(measurements_cam2, views);

SmartFactor::shared_ptr smartFactor3(
new SmartFactor(model, sharedK, boost::none, params)); // HESSIAN, by default
new SmartFactor(model, sharedK, params)); // HESSIAN, by default
smartFactor3->add(measurements_cam3, views);

NonlinearFactorGraph graph;
Expand Down Expand Up @@ -934,11 +934,11 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac
params.setDegeneracyMode(gtsam::HANDLE_INFINITY);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedK2, boost::none, params));
new SmartFactor(model, sharedK2, params));
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedK2, boost::none, params));
new SmartFactor(model, sharedK2, params));
smartFactor2->add(measurements_cam2, views);

const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
Expand Down Expand Up @@ -992,15 +992,15 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor )
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor2->add(measurements_cam2, views);

SmartFactor::shared_ptr smartFactor3(
new SmartFactor(model, sharedK, boost::none, params));
new SmartFactor(model, sharedK, params));
smartFactor3->add(measurements_cam3, views);

const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
Expand Down Expand Up @@ -1187,7 +1187,7 @@ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) {
using namespace bundlerPose;
SmartProjectionParams params;
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
SmartFactor factor(model, sharedBundlerK, boost::none, params);
SmartFactor factor(model, sharedBundlerK, params);
factor.add(measurement1, x1);
}

Expand Down Expand Up @@ -1276,15 +1276,15 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) {
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);

SmartFactor::shared_ptr smartFactor1(
new SmartFactor(model, sharedBundlerK, boost::none, params));
new SmartFactor(model, sharedBundlerK, params));
smartFactor1->add(measurements_cam1, views);

SmartFactor::shared_ptr smartFactor2(
new SmartFactor(model, sharedBundlerK, boost::none, params));
new SmartFactor(model, sharedBundlerK, params));
smartFactor2->add(measurements_cam2, views);

SmartFactor::shared_ptr smartFactor3(
new SmartFactor(model, sharedBundlerK, boost::none, params));
new SmartFactor(model, sharedBundlerK, params));
smartFactor3->add(measurements_cam3, views);

const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
Expand Down Expand Up @@ -1345,7 +1345,7 @@ TEST(SmartProjectionPoseFactor, serialize) {
using namespace gtsam::serializationTestHelpers;
SmartProjectionParams params;
params.setRankTolerance(rankTol);
SmartFactor factor(model, sharedK, boost::none, params);
SmartFactor factor(model, sharedK, params);

EXPECT(equalsObj(factor));
EXPECT(equalsXML(factor));
Expand Down

0 comments on commit 20736b6

Please sign in to comment.