12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

deprecated SmartProjectionFactor constructor with offset

release/4.3a0
Frank Dellaert 2019-08-08 11:53:05 -04:00
parent 0eef77ff36
commit 20736b6f14
5 changed files with 86 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam.h
View File

@ -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,

37
gtsam/slam/SmartProjectionFactor.h
View File

@ -79,15 +79,15 @@ public:
/**
* 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() {
@ -443,7 +443,26 @@ public:
/** 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;

29
gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -66,16 +66,31 @@ public:
/**
* 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 */

67
gtsam/slam/tests/testSmartProjectionFactor.cpp
View File

@ -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);
}
/* ************************************************************************* */
@ -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);
}
@ -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);
@ -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 =
@ -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");

58
gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -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);
}
/* ************************************************************************* */
@ -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);
}
@ -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);
@ -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);
@ -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);
@ -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);
@ -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;
@ -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);
@ -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);
@ -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);
}
@ -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);
@ -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));