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

Merge branch 'feature/cameraTemplateForAllSmartFactors' into feature/sphericalCamera 

# Conflicts:
#	gtsam/slam/tests/testSmartProjectionRigFactor.cpp
release/4.3a0
lcarlone 2021-11-07 18:47:59 -05:00
parent 78a4075a54 620f9cb99f
commit 52fb88abe6
4 changed files with 167 additions and 227 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -64,9 +64,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
/// vector of keys (one for each observation) with potentially repeated keys /// vector of keys (one for each observation) with potentially repeated keys
KeyVector nonUniqueKeys_; KeyVector nonUniqueKeys_;
/// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each /// cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
/// camera) boost::shared_ptr<typename Base::Cameras> cameraRig_;
typename Base::Cameras cameraRig_;
/// vector of camera Ids (one for each observation, in the same order), /// vector of camera Ids (one for each observation, in the same order),
/// identifying which camera took the measurement /// identifying which camera took the measurement
@ -93,7 +92,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
* @param params parameters for the smart projection factors * @param params parameters for the smart projection factors
*/ */
SmartProjectionRigFactor( SmartProjectionRigFactor(
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<Cameras>& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) { : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// throw exception if configuration is not supported by this factor // throw exception if configuration is not supported by this factor
@ -107,29 +107,6 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
"linearizationMode must be set to HESSIAN"); "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 */ /** Virtual destructor */
~SmartProjectionRigFactor() override = default; ~SmartProjectionRigFactor() override = default;
@ -177,7 +154,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
"SmartProjectionRigFactor: " "SmartProjectionRigFactor: "
"trying to add inconsistent inputs"); "trying to add inconsistent inputs");
} }
if (cameraIds.size() == 0 && cameraRig_.size() > 1) { if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionRigFactor: " "SmartProjectionRigFactor: "
"camera rig includes multiple camera " "camera rig includes multiple camera "
@ -194,7 +171,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; } const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
/// return the calibration object /// return the calibration object
const Cameras& cameraRig() const { return cameraRig_; } const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
/// return the calibration object /// return the calibration object
const FastVector<size_t>& cameraIds() const { return cameraIds_; } const FastVector<size_t>& cameraIds() const { return cameraIds_; }
@ -212,7 +189,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
std::cout << "-- Measurement nr " << i << std::endl; std::cout << "-- Measurement nr " << i << std::endl;
std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl; std::cout << "key: " << keyFormatter(nonUniqueKeys_[i]) << std::endl;
std::cout << "cameraId: " << cameraIds_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl;
cameraRig_[cameraIds_[i]].print("camera in rig:\n"); (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
} }
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }
@ -221,7 +198,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override { bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
const This* e = dynamic_cast<const This*>(&p); const This* e = dynamic_cast<const This*>(&p);
return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() && return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
cameraRig_.equals(e->cameraRig()) && cameraRig_->equals(*(e->cameraRig())) &&
std::equal(cameraIds_.begin(), cameraIds_.end(), std::equal(cameraIds_.begin(), cameraIds_.end(),
e->cameraIds().begin()); e->cameraIds().begin());
} }
@ -236,7 +213,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
typename Base::Cameras cameras; typename Base::Cameras cameras;
cameras.reserve(nonUniqueKeys_.size()); // preallocate cameras.reserve(nonUniqueKeys_.size()); // preallocate
for (size_t i = 0; i < nonUniqueKeys_.size(); i++) { for (size_t i = 0; i < nonUniqueKeys_.size(); i++) {
const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
const Pose3 world_P_sensor_i = const Pose3 world_P_sensor_i =
values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
* camera_i.pose(); // = body_P_cam_i * camera_i.pose(); // = body_P_cam_i
@ -275,7 +252,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
} else { // valid result: compute jacobians } else { // valid result: compute jacobians
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E); b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
for (size_t i = 0; i < Fs.size(); i++) { for (size_t i = 0; i < Fs.size(); i++) {
const Pose3& body_P_sensor = cameraRig_[cameraIds_[i]].pose(); const Pose3& body_P_sensor = (*cameraRig_)[cameraIds_[i]].pose();
const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse(); const Pose3 world_P_body = cameras[i].pose() * body_P_sensor.inverse();
Eigen::Matrix<double, DimPose, DimPose> H; Eigen::Matrix<double, DimPose, DimPose> H;
world_P_body.compose(body_P_sensor, H); world_P_body.compose(body_P_sensor, H);
@ -380,9 +357,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
template <class ARCHIVE> template <class ARCHIVE>
void serialize(ARCHIVE& ar, const unsigned int /*version*/) { void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base); ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
//ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_); // ar& BOOST_SERIALIZATION_NVP(nonUniqueKeys_);
// ar& BOOST_SERIALIZATION_NVP(cameraRig_); // ar& BOOST_SERIALIZATION_NVP(cameraRig_);
//ar& BOOST_SERIALIZATION_NVP(cameraIds_); // ar& BOOST_SERIALIZATION_NVP(cameraIds_);
} }
}; };
// end of class declaration // end of class declaration

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

@ -68,8 +68,8 @@ SmartProjectionParams params(
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor) { TEST(SmartProjectionRigFactor, Constructor) {
using namespace vanillaRig; using namespace vanillaRig;
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartRigFactor::shared_ptr factor1( SmartRigFactor::shared_ptr factor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
} }
@ -77,7 +77,7 @@ TEST(SmartProjectionRigFactor, Constructor) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor2) { TEST(SmartProjectionRigFactor, Constructor2) {
using namespace vanillaRig; using namespace vanillaRig;
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
SmartProjectionParams params2( SmartProjectionParams params2(
gtsam::HESSIAN, gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
@ -88,8 +88,8 @@ TEST(SmartProjectionRigFactor, Constructor2) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor3) { TEST(SmartProjectionRigFactor, Constructor3) {
using namespace vanillaRig; using namespace vanillaRig;
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartRigFactor::shared_ptr factor1( SmartRigFactor::shared_ptr factor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
factor1->add(measurement1, x1, cameraId1); factor1->add(measurement1, x1, cameraId1);
@ -98,8 +98,8 @@ TEST(SmartProjectionRigFactor, Constructor3) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, Constructor4) { TEST(SmartProjectionRigFactor, Constructor4) {
using namespace vanillaRig; using namespace vanillaRig;
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartProjectionParams params2( SmartProjectionParams params2(
gtsam::HESSIAN, gtsam::HESSIAN,
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
@ -108,22 +108,11 @@ TEST(SmartProjectionRigFactor, Constructor4) {
factor1.add(measurement1, x1, cameraId1); 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) { TEST(SmartProjectionRigFactor, Equals) {
using namespace vanillaRig; using namespace vanillaRig;
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartRigFactor::shared_ptr factor1( SmartRigFactor::shared_ptr factor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -136,8 +125,8 @@ TEST(SmartProjectionRigFactor, Equals) {
CHECK(assert_equal(*factor1, *factor2)); CHECK(assert_equal(*factor1, *factor2));
SmartRigFactor::shared_ptr factor3( SmartRigFactor::shared_ptr factor3(
new SmartRigFactor(model, Camera(Pose3::identity(), sharedK), params)); new SmartRigFactor(model, cameraRig, params));
factor3->add(measurement1, x1); // now use default factor3->add(measurement1, x1); // now use default camera ID
CHECK(assert_equal(*factor1, *factor3)); CHECK(assert_equal(*factor1, *factor3));
} }
@ -150,7 +139,10 @@ TEST(SmartProjectionRigFactor, noiseless) {
Point2 level_uv = level_camera.project(landmark1); Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1); Point2 level_uv_right = level_camera_right.project(landmark1);
SmartRigFactor factor(model, Camera(Pose3::identity(), sharedK), params); boost::shared_ptr<Cameras> cameraRig(new Cameras()); // 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, x1); // both taken from the same camera
factor.add(level_uv_right, x2); factor.add(level_uv_right, x2);
@ -204,8 +196,8 @@ TEST(SmartProjectionRigFactor, noiseless) {
TEST(SmartProjectionRigFactor, noisy) { TEST(SmartProjectionRigFactor, noisy) {
using namespace vanillaRig; using namespace vanillaRig;
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
// Project two landmarks into two cameras // Project two landmarks into two cameras
Point2 pixelError(0.2, 0.2); Point2 pixelError(0.2, 0.2);
@ -248,8 +240,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
// create arbitrary body_T_sensor (transforms from sensor to body) // create arbitrary body_T_sensor (transforms from sensor to body)
Pose3 body_T_sensor = Pose3 body_T_sensor =
Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1)); Pose3(Rot3::Ypr(-M_PI / 2, 0., -M_PI / 2), Point3(1, 1, 1));
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(body_T_sensor, sharedK)); cameraRig->push_back(Camera(body_T_sensor, sharedK));
// These are the poses we want to estimate, from camera measurements // These are the poses we want to estimate, from camera measurements
const Pose3 sensor_T_body = body_T_sensor.inverse(); const Pose3 sensor_T_body = body_T_sensor.inverse();
@ -333,10 +325,10 @@ TEST(SmartProjectionRigFactor, smartFactorWithMultipleCameras) {
Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1)); Pose3(Rot3::Ypr(-M_PI / 5, 0., -M_PI / 2), Point3(0, 0, 1));
Pose3 body_T_sensor3 = Pose3::identity(); Pose3 body_T_sensor3 = Pose3::identity();
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(body_T_sensor1, sharedK)); cameraRig->push_back(Camera(body_T_sensor1, sharedK));
cameraRig.push_back(Camera(body_T_sensor2, sharedK)); cameraRig->push_back(Camera(body_T_sensor2, sharedK));
cameraRig.push_back(Camera(body_T_sensor3, sharedK)); cameraRig->push_back(Camera(body_T_sensor3, sharedK));
// These are the poses we want to estimate, from camera measurements // These are the poses we want to estimate, from camera measurements
const Pose3 sensor_T_body1 = body_T_sensor1.inverse(); const Pose3 sensor_T_body1 = body_T_sensor1.inverse();
@ -414,8 +406,8 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
using namespace vanillaPose2; using namespace vanillaPose2;
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3; Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
// Project three landmarks into three cameras // Project three landmarks into three cameras
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
@ -502,8 +494,11 @@ TEST(SmartProjectionRigFactor, Factors) {
KeyVector views{x1, x2}; KeyVector views{x1, x2};
FastVector<size_t> cameraIds{0, 0}; FastVector<size_t> cameraIds{0, 0};
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>( SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
model, Camera(Pose3::identity(), sharedK), params); model, cameraRig, params);
smartFactor1->add(measurements_cam1, smartFactor1->add(measurements_cam1,
views); // we have a single camera so use default cameraIds views); // we have a single camera so use default cameraIds
@ -581,14 +576,9 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
std::vector<boost::shared_ptr<Cal3_S2>> sharedKs;
sharedKs.push_back(sharedK);
sharedKs.push_back(sharedK);
sharedKs.push_back(sharedK);
// create smart factor // create smart factor
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -655,8 +645,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false); params.setEnableEPI(false);
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -726,8 +716,8 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -792,8 +782,8 @@ TEST(SmartProjectionRigFactor, CheckHessian) {
params.setRankTolerance(10); params.setRankTolerance(10);
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -868,8 +858,8 @@ TEST(SmartProjectionRigFactor, Hessian) {
measurements_cam1.push_back(cam1_uv1); measurements_cam1.push_back(cam1_uv1);
measurements_cam1.push_back(cam2_uv1); measurements_cam1.push_back(cam2_uv1);
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK2)); cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
FastVector<size_t> cameraIds{0, 0}; FastVector<size_t> cameraIds{0, 0};
SmartProjectionParams params( SmartProjectionParams params(
@ -898,8 +888,8 @@ TEST(SmartProjectionRigFactor, Hessian) {
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) { TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
using namespace bundlerPose; using namespace bundlerPose;
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
SmartProjectionParams params; SmartProjectionParams params;
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
@ -926,8 +916,8 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) {
KeyVector views{x1, x2, x3}; KeyVector views{x1, x2, x3};
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK)); cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -997,8 +987,8 @@ TEST(SmartProjectionRigFactor,
// create inputs // create inputs
KeyVector keys{x1, x2, x3, x1}; KeyVector keys{x1, x2, x3, x1};
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0, 0};
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
@ -1125,8 +1115,8 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
// create inputs // create inputs
KeyVector keys{x1, x2, x3}; KeyVector keys{x1, x2, x3};
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0}; FastVector<size_t> cameraIdsRedundant{0, 0, 0, 0};
@ -1211,8 +1201,8 @@ TEST(SmartProjectionRigFactor, timing) {
Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple); Camera cam1(pose1, sharedKSimple), cam2(pose2, sharedKSimple);
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3::identity();
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple)); cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
// one landmarks 1m in front of camera // one landmarks 1m in front of camera
Point3 landmark1(0, 0, 10); Point3 landmark1(0, 0, 10);
@ -1276,8 +1266,8 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
keys.push_back(x2); keys.push_back(x2);
keys.push_back(x3); keys.push_back(x3);
Cameras cameraRig; // single camera in the rig boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(Pose3::identity(), emptyK)); cameraRig->push_back(Camera(Pose3::identity(), emptyK));
SmartProjectionParams params( SmartProjectionParams params(
gtsam::HESSIAN, gtsam::HESSIAN,
@ -1369,8 +1359,9 @@ TEST(SmartProjectionFactorP, timing_sphericalCamera) {
size_t nrTests = 1000; size_t nrTests = 1000;
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
CameraSet<SphericalCamera> cameraRig; // single camera in the rig boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
cameraRig.push_back(SphericalCamera(body_P_sensorId, emptyK)); new CameraSet<SphericalCamera>()); // single camera in the rig
cameraRig->push_back(SphericalCamera(body_P_sensorId, emptyK));
SmartProjectionRigFactor<SphericalCamera>::shared_ptr smartFactorP( SmartProjectionRigFactor<SphericalCamera>::shared_ptr smartFactorP(
new SmartProjectionRigFactor<SphericalCamera>(model, cameraRig, new SmartProjectionRigFactor<SphericalCamera>(model, cameraRig,
@ -1387,8 +1378,9 @@ TEST(SmartProjectionFactorP, timing_sphericalCamera) {
} }
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
cameraRig.push_back(PinholePose<Cal3_S2>(body_P_sensorId, sharedKSimple)); new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
cameraRig->push_back(PinholePose<Cal3_S2>(body_P_sensorId, sharedKSimple));
SmartProjectionRigFactor<PinholePose<Cal3_S2>>::shared_ptr smartFactorP2( SmartProjectionRigFactor<PinholePose<Cal3_S2>>::shared_ptr smartFactorP2(
new SmartProjectionRigFactor<PinholePose<Cal3_S2>>(model, cameraRig, new SmartProjectionRigFactor<PinholePose<Cal3_S2>>(model, cameraRig,
@ -1429,8 +1421,9 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params.setRankTolerance(1); params.setRankTolerance(1);
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK)); new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -1466,8 +1459,9 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params.setRankTolerance(0.1); params.setRankTolerance(0.1);
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK)); new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -1502,8 +1496,9 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
params.setRankTolerance(0.01); params.setRankTolerance(0.01);
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK)); new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
SmartRigFactor::shared_ptr smartFactor1( SmartRigFactor::shared_ptr smartFactor1(
new SmartRigFactor(model, cameraRig, params)); new SmartRigFactor(model, cameraRig, params));
@ -1540,8 +1535,9 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
Camera cam1(poseA); Camera cam1(poseA);
Camera cam2(poseB); Camera cam2(poseB);
CameraSet<SphericalCamera> cameraRig; // single camera in the rig boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
cameraRig.push_back(SphericalCamera(Pose3::identity(), emptyK)); new CameraSet<SphericalCamera>()); // single camera in the rig
cameraRig->push_back(SphericalCamera(Pose3::identity(), emptyK));
// TRIANGULATION TEST WITH DEFAULT RANK TOL // TRIANGULATION TEST WITH DEFAULT RANK TOL
{ // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a { // rankTol = 1 or 0.1 gives a degenerate point, which is undesirable for a
@ -1599,57 +1595,6 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
} }
} }
/* ************************************************************************* */
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained,
// "gtsam_noiseModel_Constrained");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal,
// "gtsam_noiseModel_Diagonal");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian,
// "gtsam_noiseModel_Gaussian");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
// BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic,
// "gtsam_noiseModel_Isotropic");
// BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
// BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
//
// SERIALIZATION TEST FAILS: to be fixed
// TEST(SmartProjectionFactorP, serialize) {
// using namespace vanillaPose;
// using namespace gtsam::serializationTestHelpers;
// SmartProjectionParams params(
// gtsam::HESSIAN,
// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig
// factors
// params.setRankTolerance(rankTol);
//
// CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
// cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
//
// SmartRigFactor factor(model, cameraRig, params);
//
// EXPECT(equalsObj(factor));
// EXPECT(equalsXML(factor));
// EXPECT(equalsBinary(factor));
//}
//
// TEST(SmartProjectionFactorP, serialize2) {
// using namespace vanillaPose;
// using namespace gtsam::serializationTestHelpers;
// SmartProjectionParams params(
// gtsam::HESSIAN,
// gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
// params.setRankTolerance(rankTol);
//
// Cameras cameraRig; // single camera in the rig
// cameraRig.push_back( Camera(Pose3::identity(), sharedK) );
//
// SmartRigFactor factor(model, cameraRig, params);
//
// EXPECT(equalsObj(factor));
// EXPECT(equalsXML(factor));
// EXPECT(equalsBinary(factor));
//}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

39
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -61,7 +61,7 @@ class SmartProjectionPoseFactorRollingShutter
/// one or more cameras taking observations (fixed poses wrt body + fixed /// one or more cameras taking observations (fixed poses wrt body + fixed
/// intrinsics) /// intrinsics)
typename Base::Cameras cameraRig_; boost::shared_ptr<typename Base::Cameras> cameraRig_;
/// vector of camera Ids (one for each observation, in the same order), /// vector of camera Ids (one for each observation, in the same order),
/// identifying which camera took the measurement /// identifying which camera took the measurement
@ -97,7 +97,8 @@ class SmartProjectionPoseFactorRollingShutter
* @param params internal parameters of the smart factors * @param params internal parameters of the smart factors
*/ */
SmartProjectionPoseFactorRollingShutter( SmartProjectionPoseFactorRollingShutter(
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig, const SharedNoiseModel& sharedNoiseModel,
const boost::shared_ptr<Cameras>& cameraRig,
const SmartProjectionParams& params = SmartProjectionParams()) const SmartProjectionParams& params = SmartProjectionParams())
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) { : Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
// throw exception if configuration is not supported by this factor // throw exception if configuration is not supported by this factor
@ -111,28 +112,6 @@ class SmartProjectionPoseFactorRollingShutter
"linearizationMode must be set to HESSIAN"); "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 */ /** Virtual destructor */
~SmartProjectionPoseFactorRollingShutter() override = default; ~SmartProjectionPoseFactorRollingShutter() override = default;
@ -199,7 +178,7 @@ class SmartProjectionPoseFactorRollingShutter
"SmartProjectionPoseFactorRollingShutter: " "SmartProjectionPoseFactorRollingShutter: "
"trying to add inconsistent inputs"); "trying to add inconsistent inputs");
} }
if (cameraIds.size() == 0 && cameraRig_.size() > 1) { if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
throw std::runtime_error( throw std::runtime_error(
"SmartProjectionPoseFactorRollingShutter: " "SmartProjectionPoseFactorRollingShutter: "
"camera rig includes multiple camera " "camera rig includes multiple camera "
@ -224,7 +203,7 @@ class SmartProjectionPoseFactorRollingShutter
const std::vector<double>& alphas() const { return alphas_; } const std::vector<double>& alphas() const { return alphas_; }
/// return the calibration object /// return the calibration object
const Cameras& cameraRig() const { return cameraRig_; } const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
/// return the calibration object /// return the calibration object
const FastVector<size_t>& cameraIds() const { return cameraIds_; } const FastVector<size_t>& cameraIds() const { return cameraIds_; }
@ -246,7 +225,7 @@ class SmartProjectionPoseFactorRollingShutter
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl; << keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
std::cout << " alpha: " << alphas_[i] << std::endl; std::cout << " alpha: " << alphas_[i] << std::endl;
std::cout << "cameraId: " << cameraIds_[i] << std::endl; std::cout << "cameraId: " << cameraIds_[i] << std::endl;
cameraRig_[cameraIds_[i]].print("camera in rig:\n"); (*cameraRig_)[cameraIds_[i]].print("camera in rig:\n");
} }
Base::print("", keyFormatter); Base::print("", keyFormatter);
} }
@ -275,7 +254,7 @@ class SmartProjectionPoseFactorRollingShutter
} }
return e && Base::equals(p, tol) && alphas_ == e->alphas() && return e && Base::equals(p, tol) && alphas_ == e->alphas() &&
keyPairsEqual && cameraRig_.equals(e->cameraRig()) && keyPairsEqual && cameraRig_->equals(*(e->cameraRig())) &&
std::equal(cameraIds_.begin(), cameraIds_.end(), std::equal(cameraIds_.begin(), cameraIds_.end(),
e->cameraIds().begin()); e->cameraIds().begin());
} }
@ -297,7 +276,7 @@ class SmartProjectionPoseFactorRollingShutter
double interpolationFactor = alphas_[i]; double interpolationFactor = alphas_[i];
const Pose3& w_P_body = const Pose3& w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor); interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor);
const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
const Pose3& body_P_cam = camera_i.pose(); const Pose3& body_P_cam = camera_i.pose();
const Pose3& w_P_cam = w_P_body.compose(body_P_cam); const Pose3& w_P_cam = w_P_body.compose(body_P_cam);
cameras.emplace_back(w_P_cam, cameras.emplace_back(w_P_cam,
@ -350,7 +329,7 @@ class SmartProjectionPoseFactorRollingShutter
auto w_P_body = auto w_P_body =
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor, interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
dInterpPose_dPoseBody1, dInterpPose_dPoseBody2); dInterpPose_dPoseBody1, dInterpPose_dPoseBody2);
const typename Base::Camera& camera_i = cameraRig_[cameraIds_[i]]; const typename Base::Camera& camera_i = (*cameraRig_)[cameraIds_[i]];
auto body_P_cam = camera_i.pose(); auto body_P_cam = camera_i.pose();
auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose); auto w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
typename Base::Camera camera( typename Base::Camera camera(

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

@ -87,22 +87,28 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr factor1( SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); new SmartFactorRS(model, cameraRig, params));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) { TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
params.setRankTolerance(rankTol); params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, Camera(Pose3::identity(), sharedK), params); SmartFactorRS factor1(model, cameraRig, params);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, add) { TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPoseRS; using namespace vanillaPoseRS;
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr factor1( SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, Camera(Pose3::identity(), sharedK), params)); new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor); factor1->add(measurement1, x1, x2, interp_factor);
} }
@ -128,8 +134,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
FastVector<size_t> cameraIds{0, 0, 0}; FastVector<size_t> cameraIds{0, 0, 0};
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(body_P_sensor, sharedK)); cameraRig->push_back(Camera(body_P_sensor, sharedK));
// create by adding a batch of measurements with a bunch of calibrations // create by adding a batch of measurements with a bunch of calibrations
SmartFactorRS::shared_ptr factor2( SmartFactorRS::shared_ptr factor2(
@ -183,8 +189,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
} }
{ // create slightly different factors (different extrinsics) and show equal { // create slightly different factors (different extrinsics) and show equal
// returns false // returns false
Cameras cameraRig2; boost::shared_ptr<Cameras> cameraRig2(new Cameras());
cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK)); cameraRig2->push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
SmartFactorRS::shared_ptr factor1( SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig2, params)); new SmartFactorRS(model, cameraRig2, params));
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1); factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
@ -226,7 +232,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1); Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity(); Pose3 body_P_sensorId = Pose3::identity();
SmartFactorRS factor(model, Camera(body_P_sensorId, sharedK), params); boost::shared_ptr<Cameras> cameraRig(new Cameras());
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, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2); factor.add(level_uv_right, x2, x3, interp_factor2);
@ -301,7 +310,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1); Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorNonId = body_P_sensor; Pose3 body_P_sensorNonId = body_P_sensor;
SmartFactorRS factor(model, Camera(body_P_sensorNonId, sharedK), params); boost::shared_ptr<Cameras> cameraRig(new Cameras());
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, x1, x2, interp_factor1);
factor.add(level_uv_right, x2, x3, interp_factor2); 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_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( 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); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2( 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); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3( 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); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -463,9 +478,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(body_P_sensor, sharedK)); cameraRig->push_back(Camera(body_P_sensor, sharedK));
cameraRig.push_back(Camera(Pose3::identity(), sharedK)); cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -551,10 +566,10 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam2) {
interp_factors.push_back(interp_factor2); interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(body_T_sensor1, sharedK)); cameraRig->push_back(Camera(body_T_sensor1, sharedK));
cameraRig.push_back(Camera(body_T_sensor2, sharedK)); cameraRig->push_back(Camera(body_T_sensor2, sharedK));
cameraRig.push_back(Camera(body_T_sensor3, sharedK)); cameraRig->push_back(Camera(body_T_sensor3, sharedK));
SmartFactorRS::shared_ptr smartFactor1( SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params)); new SmartFactorRS(model, cameraRig, params));
@ -633,8 +648,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
measurements_lmk1.push_back(cam1.project(landmark1)); measurements_lmk1.push_back(cam1.project(landmark1));
measurements_lmk1.push_back(cam2.project(landmark1)); measurements_lmk1.push_back(cam2.project(landmark1));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
SmartFactorRS::shared_ptr smartFactor1( 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 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor); smartFactor1->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2
@ -728,13 +746,16 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(true); params.setEnableEPI(true);
SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS smartFactor1(model, cameraRig, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); 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); 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); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -794,13 +815,16 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false); params.setEnableEPI(false);
SmartFactorRS smartFactor1(model, Camera(Pose3::identity(), sharedK), params); boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS smartFactor1(model, cameraRig, params);
smartFactor1.add(measurements_lmk1, key_pairs, interp_factors); 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); 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); smartFactor3.add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -869,20 +893,23 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false); params.setEnableEPI(false);
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( 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); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor2( 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); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3( 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); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor4( 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); smartFactor4->add(measurements_lmk4, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); 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_factor2);
interp_factors.push_back(interp_factor3); interp_factors.push_back(interp_factor3);
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( 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); smartFactor1->add(measurements_lmk1, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), 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_factor3);
interp_factors.push_back(interp_factor1); interp_factors.push_back(interp_factor1);
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( 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); smartFactor1->add(measurements_lmk1_redundant, key_pairs, interp_factors);
Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), 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_redundant.push_back(
interp_factors.at(0)); // we readd the first interp factor interp_factors.at(0)); // we readd the first interp factor
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1( 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, smartFactor1->add(measurements_lmk1_redundant, key_pairs_redundant,
interp_factors_redundant); interp_factors_redundant);
SmartFactorRS::shared_ptr smartFactor2( 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); smartFactor2->add(measurements_lmk2, key_pairs, interp_factors);
SmartFactorRS::shared_ptr smartFactor3( 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); smartFactor3->add(measurements_lmk3, key_pairs, interp_factors);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
@ -1313,8 +1349,11 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
size_t nrTests = 10000; size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) { for (size_t i = 0; i < nrTests; i++) {
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS( 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 double interp_factor = 0; // equivalent to measurement taken at pose 1
smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor); smartFactorRS->add(measurements_lmk1[0], x1, x2, interp_factor);
interp_factor = 1; // equivalent to measurement taken at pose 2 interp_factor = 1; // equivalent to measurement taken at pose 2
@ -1391,8 +1430,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
params.setRankTolerance(0.1); params.setRankTolerance(0.1);
Cameras cameraRig; boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig.push_back(Camera(Pose3::identity(), emptyK)); cameraRig->push_back(Camera(Pose3::identity(), emptyK));
SmartFactorRS_spherical::shared_ptr smartFactor1( SmartFactorRS_spherical::shared_ptr smartFactor1(
new SmartFactorRS_spherical(model, cameraRig, params)); new SmartFactorRS_spherical(model, cameraRig, params));