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

got rid of second constructor

release/4.3a0
lcarlone 2021-11-07 18:01:28 -05:00
parent c105aa4e1e
commit 2c2e43ee5b
4 changed files with 76 additions and 87 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -105,29 +105,6 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
"linearizationMode must be set to HESSIAN");
}
/**
* Constructor
* @param sharedNoiseModel isotropic noise model for the 2D feature
* measurements
* @param camera single camera (fixed poses wrt body and intrinsics)
* @param params parameters for the smart projection factors
*/
SmartProjectionRigFactor(
const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) {
// throw exception if configuration is not supported by this factor
if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"degeneracyMode must be set to ZERO_ON_DEGENERACY");
if (Base::params_.linearizationMode != gtsam::HESSIAN)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"linearizationMode must be set to HESSIAN");
cameraRig_.push_back(camera);
}
/** Virtual destructor */
~SmartProjectionRigFactor() override = default;

27
gtsam/slam/tests/testSmartProjectionRigFactor.cpp
View File

@ -29,7 +29,7 @@
#include <iostream>
#include "smartFactorScenarios.h"
#define DISABLE_TIMING
//#define DISABLE_TIMING
using namespace boost::assign;
using namespace std::placeholders;
@ -110,17 +110,6 @@ TEST(SmartProjectionRigFactor, Constructor4) {
factor1.add(measurement1, x1, cameraId1);
}
/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor5) {
using namespace vanillaRig;
SmartProjectionParams params2(
gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params2.setRankTolerance(rankTol);
SmartRigFactor factor1(model, Camera(Pose3::identity(), sharedK), params2);
factor1.add(measurement1, x1, cameraId1);
}
/* ************************************************************************* */
TEST(SmartProjectionRigFactor, Equals) {
using namespace vanillaRig;
@ -138,8 +127,8 @@ TEST(SmartProjectionRigFactor, Equals) {
CHECK(assert_equal(*factor1, *factor2));
SmartRigFactor::shared_ptr factor3(
new SmartRigFactor(model, Camera(Pose3::identity(), sharedK), params));
factor3->add(measurement1, x1); // now use default
new SmartRigFactor(model, cameraRig, params));
factor3->add(measurement1, x1); // now use default camera ID
CHECK(assert_equal(*factor1, *factor3));
}
@ -152,7 +141,10 @@ TEST(SmartProjectionRigFactor, noiseless) {
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK), params);
Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartRigFactor factor(model, cameraRig, params);
factor.add(level_uv, x1); // both taken from the same camera
factor.add(level_uv_right, x2);
@ -504,8 +496,11 @@ TEST(SmartProjectionRigFactor, Factors) {
KeyVector views{x1, x2};
FastVector<size_t> cameraIds{0, 0};
Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
model, Camera(Pose3::identity(), sharedK), params);
model, cameraRig, params);
smartFactor1->add(measurements_cam1,
views); // we have a single camera so use default cameraIds

22
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -109,28 +109,6 @@ class SmartProjectionPoseFactorRollingShutter
"linearizationMode must be set to HESSIAN");
}
/**
* Constructor
* @param Isotropic measurement noise
* @param camera single camera (fixed poses wrt body and intrinsics)
* @param params internal parameters of the smart factors
*/
SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel, const Camera& camera,
const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params) {
// throw exception if configuration is not supported by this factor
if (Base::params_.degeneracyMode != gtsam::ZERO_ON_DEGENERACY)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"degeneracyMode must be set to ZERO_ON_DEGENERACY");
if (Base::params_.linearizationMode != gtsam::HESSIAN)
throw std::runtime_error(
"SmartProjectionRigFactor: "
"linearizationMode must be set to HESSIAN");
cameraRig_.push_back(camera);
}
/** Virtual destructor */
~SmartProjectionPoseFactorRollingShutter() override = default;

91
gtsam_unstable/slam/tests/testSmartProjectionPoseFactorRollingShutter.cpp
View File

@ -29,7 +29,7 @@
#include <iostream>
#include "gtsam/slam/tests/smartFactorScenarios.h"
#define DISABLE_TIMING
//#define DISABLE_TIMING
using namespace gtsam;
using namespace boost::assign;
@ -87,22 +87,28 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
using namespace vanillaPoseRS;
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
}
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
using namespace vanillaPoseRS;
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params);
SmartFactorRS factor1(model, cameraRig, params);
}
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPoseRS;
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor);
}
@ -226,7 +232,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity();
SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params);
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorId, sharedK));
SmartFactorRS factor(model, cameraRig, params);
factor.add(level_uv, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2);
@ -301,7 +310,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorNonId = body_P_sensor;
SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params);
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorNonId, sharedK));
SmartFactorRS factor(model, cameraRig, params);
factor.add(level_uv, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2);
@ -392,16 +404,19 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -633,8 +648,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
measurements_lmk1.push_back(cam1.project(landmark1));
measurements_lmk1.push_back(cam2.project(landmark1));
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(body_P_sensorId, sharedKSimple), params));
new SmartFactorRS(model, cameraRig, params));
double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2
@ -728,13 +746,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(true);
SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS smartFactor1(model, cameraRig, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
SmartFactorRS smartFactor2(model, cameraRig, params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
SmartFactorRS smartFactor3(model, cameraRig, params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -794,13 +815,16 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false);
SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS smartFactor1(model, cameraRig, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS smartFactor2(model, Camera(Pose3::identity(), sharedK), params);
SmartFactorRS smartFactor2(model, cameraRig, params);
smartFactor2.add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS smartFactor3(model, Camera(Pose3::identity(), sharedK), params);
SmartFactorRS smartFactor3(model, cameraRig, params);
smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -869,20 +893,23 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor4(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -933,8 +960,11 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1071,8 +1101,11 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor3);
interp_factors.push_back(interp_factor1);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100),
@ -1227,17 +1260,20 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors_redundant.push_back(
interp_factors.at(0)); // we readd the first interp factor
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
interp_factors_redundant);
SmartFactorRS::shared_ptr smartFactor2(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params));
new SmartFactorRS(model, cameraRig, params));
smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1313,8 +1349,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) {
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple));
SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
model, Camera(body_P_sensorId, sharedKSimple), params));
model, cameraRig, params));
double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2