Merge branch 'feature/cameraTemplateForAllSmartFactors' into feature/sphericalCamera
# Conflicts: # gtsam/slam/tests/testSmartProjectionRigFactor.cpprelease/4.3a0
lcarlone
commit
52fb88abe6
|
|
@ -64,9 +64,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
/// vector of keys (one for each observation) with potentially repeated keys
|
||||
KeyVector nonUniqueKeys_;
|
||||
|
||||
/// cameras in the rig (fixed poses wrt body + fixed intrinsics, for each
|
||||
/// camera)
|
||||
typename Base::Cameras cameraRig_;
|
||||
/// cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
|
||||
boost::shared_ptr<typename Base::Cameras> cameraRig_;
|
||||
|
||||
/// vector of camera Ids (one for each observation, in the same order),
|
||||
/// identifying which camera took the measurement
|
||||
|
|
@ -93,7 +92,8 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
* @param params parameters for the smart projection factors
|
||||
*/
|
||||
SmartProjectionRigFactor(
|
||||
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
|
||||
const SharedNoiseModel& sharedNoiseModel,
|
||||
const boost::shared_ptr<Cameras>& cameraRig,
|
||||
const SmartProjectionParams& params = SmartProjectionParams())
|
||||
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
|
||||
// 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");
|
||||
}
|
||||
|
||||
/**
|
||||
* 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;
|
||||
|
||||
|
|
@ -177,7 +154,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
"SmartProjectionRigFactor: "
|
||||
"trying to add inconsistent inputs");
|
||||
}
|
||||
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
|
||||
if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
|
||||
throw std::runtime_error(
|
||||
"SmartProjectionRigFactor: "
|
||||
"camera rig includes multiple camera "
|
||||
|
|
@ -194,7 +171,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
const KeyVector& nonUniqueKeys() const { return nonUniqueKeys_; }
|
||||
|
||||
/// return the calibration object
|
||||
const Cameras& cameraRig() const { return cameraRig_; }
|
||||
const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
|
||||
|
||||
/// return the calibration object
|
||||
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 << "key: " << keyFormatter(nonUniqueKeys_[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);
|
||||
}
|
||||
|
|
@ -221,7 +198,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
bool equals(const NonlinearFactor& p, double tol = 1e-9) const override {
|
||||
const This* e = dynamic_cast<const This*>(&p);
|
||||
return e && Base::equals(p, tol) && nonUniqueKeys_ == e->nonUniqueKeys() &&
|
||||
cameraRig_.equals(e->cameraRig()) &&
|
||||
cameraRig_->equals(*(e->cameraRig())) &&
|
||||
std::equal(cameraIds_.begin(), cameraIds_.end(),
|
||||
e->cameraIds().begin());
|
||||
}
|
||||
|
|
@ -236,7 +213,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
typename Base::Cameras cameras;
|
||||
cameras.reserve(nonUniqueKeys_.size()); // preallocate
|
||||
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 =
|
||||
values.at<Pose3>(nonUniqueKeys_[i]) // = world_P_body
|
||||
* camera_i.pose(); // = body_P_cam_i
|
||||
|
|
@ -275,7 +252,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
} else { // valid result: compute jacobians
|
||||
b = -cameras.reprojectionError(*this->result_, this->measured_, Fs, E);
|
||||
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();
|
||||
Eigen::Matrix<double, DimPose, DimPose> H;
|
||||
world_P_body.compose(body_P_sensor, H);
|
||||
|
|
@ -380,9 +357,9 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
|
|||
template <class ARCHIVE>
|
||||
void serialize(ARCHIVE& ar, const unsigned int /*version*/) {
|
||||
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(cameraIds_);
|
||||
// ar& BOOST_SERIALIZATION_NVP(cameraIds_);
|
||||
}
|
||||
};
|
||||
// end of class declaration
|
||||
|
|
|
|||
|
|
@ -68,8 +68,8 @@ SmartProjectionParams params(
|
|||
/* ************************************************************************* */
|
||||
TEST(SmartProjectionRigFactor, Constructor) {
|
||||
using namespace vanillaRig;
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
SmartRigFactor::shared_ptr factor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
}
|
||||
|
|
@ -77,7 +77,7 @@ TEST(SmartProjectionRigFactor, Constructor) {
|
|||
/* ************************************************************************* */
|
||||
TEST(SmartProjectionRigFactor, Constructor2) {
|
||||
using namespace vanillaRig;
|
||||
Cameras cameraRig;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
SmartProjectionParams params2(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
|
|
@ -88,8 +88,8 @@ TEST(SmartProjectionRigFactor, Constructor2) {
|
|||
/* ************************************************************************* */
|
||||
TEST(SmartProjectionRigFactor, Constructor3) {
|
||||
using namespace vanillaRig;
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
SmartRigFactor::shared_ptr factor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
factor1->add(measurement1, x1, cameraId1);
|
||||
|
|
@ -98,8 +98,8 @@ TEST(SmartProjectionRigFactor, Constructor3) {
|
|||
/* ************************************************************************* */
|
||||
TEST(SmartProjectionRigFactor, Constructor4) {
|
||||
using namespace vanillaRig;
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
SmartProjectionParams params2(
|
||||
gtsam::HESSIAN,
|
||||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
|
|
@ -108,22 +108,11 @@ 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;
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
|
||||
SmartRigFactor::shared_ptr factor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -136,8 +125,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));
|
||||
}
|
||||
|
|
@ -150,7 +139,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);
|
||||
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_right, x2);
|
||||
|
||||
|
|
@ -204,8 +196,8 @@ TEST(SmartProjectionRigFactor, noiseless) {
|
|||
TEST(SmartProjectionRigFactor, noisy) {
|
||||
using namespace vanillaRig;
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
|
||||
// Project two landmarks into two cameras
|
||||
Point2 pixelError(0.2, 0.2);
|
||||
|
|
@ -248,8 +240,8 @@ TEST(SmartProjectionRigFactor, smartFactorWithSensorBodyTransform) {
|
|||
// 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));
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(body_T_sensor, sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(body_T_sensor, sharedK));
|
||||
|
||||
// These are the poses we want to estimate, from camera measurements
|
||||
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 body_T_sensor3 = Pose3::identity();
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(body_T_sensor1, sharedK));
|
||||
cameraRig.push_back(Camera(body_T_sensor2, sharedK));
|
||||
cameraRig.push_back(Camera(body_T_sensor3, sharedK));
|
||||
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_sensor2, sharedK));
|
||||
cameraRig->push_back(Camera(body_T_sensor3, sharedK));
|
||||
|
||||
// These are the poses we want to estimate, from camera measurements
|
||||
const Pose3 sensor_T_body1 = body_T_sensor1.inverse();
|
||||
|
|
@ -414,8 +406,8 @@ TEST(SmartProjectionRigFactor, 3poses_smart_projection_factor) {
|
|||
using namespace vanillaPose2;
|
||||
Point2Vector measurements_cam1, measurements_cam2, measurements_cam3;
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK2));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
|
||||
|
||||
// Project three landmarks into three cameras
|
||||
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
|
||||
|
|
@ -502,8 +494,11 @@ TEST(SmartProjectionRigFactor, Factors) {
|
|||
KeyVector views{x1, x2};
|
||||
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>(
|
||||
model, Camera(Pose3::identity(), sharedK), params);
|
||||
model, cameraRig, params);
|
||||
smartFactor1->add(measurements_cam1,
|
||||
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, 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
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -655,8 +645,8 @@ TEST(SmartProjectionRigFactor, landmarkDistance) {
|
|||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setEnableEPI(false);
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -726,8 +716,8 @@ TEST(SmartProjectionRigFactor, dynamicOutlierRejection) {
|
|||
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
|
||||
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -792,8 +782,8 @@ TEST(SmartProjectionRigFactor, CheckHessian) {
|
|||
params.setRankTolerance(10);
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -868,8 +858,8 @@ TEST(SmartProjectionRigFactor, Hessian) {
|
|||
measurements_cam1.push_back(cam1_uv1);
|
||||
measurements_cam1.push_back(cam2_uv1);
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK2));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK2));
|
||||
FastVector<size_t> cameraIds{0, 0};
|
||||
|
||||
SmartProjectionParams params(
|
||||
|
|
@ -898,8 +888,8 @@ TEST(SmartProjectionRigFactor, Hessian) {
|
|||
/* ************************************************************************* */
|
||||
TEST(SmartProjectionRigFactor, ConstructorWithCal3Bundler) {
|
||||
using namespace bundlerPose;
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
|
||||
|
||||
SmartProjectionParams params;
|
||||
params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY);
|
||||
|
|
@ -926,8 +916,8 @@ TEST(SmartProjectionRigFactor, Cal3Bundler) {
|
|||
|
||||
KeyVector views{x1, x2, x3};
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedBundlerK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedBundlerK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -997,8 +987,8 @@ TEST(SmartProjectionRigFactor,
|
|||
// create inputs
|
||||
KeyVector keys{x1, x2, x3, x1};
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
FastVector<size_t> cameraIds{0, 0, 0, 0};
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
|
|
@ -1125,8 +1115,8 @@ TEST(SmartProjectionRigFactor, optimization_3poses_measurementsFromSamePose) {
|
|||
|
||||
// create inputs
|
||||
KeyVector keys{x1, x2, x3};
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
FastVector<size_t> cameraIds{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);
|
||||
Pose3 body_P_sensorId = Pose3::identity();
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras()); // single camera in the rig
|
||||
cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
|
||||
|
||||
// one landmarks 1m in front of camera
|
||||
Point3 landmark1(0, 0, 10);
|
||||
|
|
@ -1276,8 +1266,8 @@ TEST(SmartProjectionFactorP, optimization_3poses_sphericalCamera) {
|
|||
keys.push_back(x2);
|
||||
keys.push_back(x3);
|
||||
|
||||
Cameras cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(Camera(Pose3::identity(), emptyK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), emptyK));
|
||||
|
||||
SmartProjectionParams params(
|
||||
gtsam::HESSIAN,
|
||||
|
|
@ -1369,8 +1359,9 @@ TEST(SmartProjectionFactorP, timing_sphericalCamera) {
|
|||
size_t nrTests = 1000;
|
||||
|
||||
for (size_t i = 0; i < nrTests; i++) {
|
||||
CameraSet<SphericalCamera> cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(SphericalCamera(body_P_sensorId, emptyK));
|
||||
boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
|
||||
new CameraSet<SphericalCamera>()); // single camera in the rig
|
||||
cameraRig->push_back(SphericalCamera(body_P_sensorId, emptyK));
|
||||
|
||||
SmartProjectionRigFactor<SphericalCamera>::shared_ptr smartFactorP(
|
||||
new SmartProjectionRigFactor<SphericalCamera>(model, cameraRig,
|
||||
|
|
@ -1387,8 +1378,9 @@ TEST(SmartProjectionFactorP, timing_sphericalCamera) {
|
|||
}
|
||||
|
||||
for (size_t i = 0; i < nrTests; i++) {
|
||||
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(PinholePose<Cal3_S2>(body_P_sensorId, sharedKSimple));
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
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(
|
||||
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
|
||||
params.setRankTolerance(1);
|
||||
|
||||
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), sharedK));
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -1466,8 +1459,9 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
|
|||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
params.setRankTolerance(0.1);
|
||||
|
||||
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -1502,8 +1496,9 @@ TEST(SmartProjectionFactorP, 2poses_rankTol) {
|
|||
gtsam::ZERO_ON_DEGENERACY); // only config that works with rig factors
|
||||
params.setRankTolerance(0.01);
|
||||
|
||||
CameraSet<PinholePose<Cal3_S2>> cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
boost::shared_ptr<CameraSet<PinholePose<Cal3_S2>>> cameraRig(
|
||||
new CameraSet<PinholePose<Cal3_S2>>()); // single camera in the rig
|
||||
cameraRig->push_back(PinholePose<Cal3_S2>(Pose3::identity(), canonicalK));
|
||||
|
||||
SmartRigFactor::shared_ptr smartFactor1(
|
||||
new SmartRigFactor(model, cameraRig, params));
|
||||
|
|
@ -1540,8 +1535,9 @@ TEST(SmartProjectionFactorP, 2poses_sphericalCamera_rankTol) {
|
|||
Camera cam1(poseA);
|
||||
Camera cam2(poseB);
|
||||
|
||||
CameraSet<SphericalCamera> cameraRig; // single camera in the rig
|
||||
cameraRig.push_back(SphericalCamera(Pose3::identity(), emptyK));
|
||||
boost::shared_ptr<CameraSet<SphericalCamera>> cameraRig(
|
||||
new CameraSet<SphericalCamera>()); // single camera in the rig
|
||||
cameraRig->push_back(SphericalCamera(Pose3::identity(), emptyK));
|
||||
|
||||
// TRIANGULATION TEST WITH DEFAULT RANK TOL
|
||||
{ // 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() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -61,7 +61,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
|
||||
/// one or more cameras taking observations (fixed poses wrt body + fixed
|
||||
/// intrinsics)
|
||||
typename Base::Cameras cameraRig_;
|
||||
boost::shared_ptr<typename Base::Cameras> cameraRig_;
|
||||
|
||||
/// vector of camera Ids (one for each observation, in the same order),
|
||||
/// identifying which camera took the measurement
|
||||
|
|
@ -97,7 +97,8 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
* @param params internal parameters of the smart factors
|
||||
*/
|
||||
SmartProjectionPoseFactorRollingShutter(
|
||||
const SharedNoiseModel& sharedNoiseModel, const Cameras& cameraRig,
|
||||
const SharedNoiseModel& sharedNoiseModel,
|
||||
const boost::shared_ptr<Cameras>& cameraRig,
|
||||
const SmartProjectionParams& params = SmartProjectionParams())
|
||||
: Base(sharedNoiseModel, params), cameraRig_(cameraRig) {
|
||||
// throw exception if configuration is not supported by this factor
|
||||
|
|
@ -111,28 +112,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;
|
||||
|
||||
|
|
@ -199,7 +178,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
"SmartProjectionPoseFactorRollingShutter: "
|
||||
"trying to add inconsistent inputs");
|
||||
}
|
||||
if (cameraIds.size() == 0 && cameraRig_.size() > 1) {
|
||||
if (cameraIds.size() == 0 && cameraRig_->size() > 1) {
|
||||
throw std::runtime_error(
|
||||
"SmartProjectionPoseFactorRollingShutter: "
|
||||
"camera rig includes multiple camera "
|
||||
|
|
@ -224,7 +203,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
const std::vector<double>& alphas() const { return alphas_; }
|
||||
|
||||
/// return the calibration object
|
||||
const Cameras& cameraRig() const { return cameraRig_; }
|
||||
const boost::shared_ptr<Cameras>& cameraRig() const { return cameraRig_; }
|
||||
|
||||
/// return the calibration object
|
||||
const FastVector<size_t>& cameraIds() const { return cameraIds_; }
|
||||
|
|
@ -246,7 +225,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
<< keyFormatter(world_P_body_key_pairs_[i].second) << std::endl;
|
||||
std::cout << " alpha: " << alphas_[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);
|
||||
}
|
||||
|
|
@ -275,7 +254,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
}
|
||||
|
||||
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(),
|
||||
e->cameraIds().begin());
|
||||
}
|
||||
|
|
@ -297,7 +276,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
double interpolationFactor = alphas_[i];
|
||||
const Pose3& w_P_body =
|
||||
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& w_P_cam = w_P_body.compose(body_P_cam);
|
||||
cameras.emplace_back(w_P_cam,
|
||||
|
|
@ -350,7 +329,7 @@ class SmartProjectionPoseFactorRollingShutter
|
|||
auto w_P_body =
|
||||
interpolate<Pose3>(w_P_body1, w_P_body2, interpolationFactor,
|
||||
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 w_P_cam = w_P_body.compose(body_P_cam, dPoseCam_dInterpPose);
|
||||
typename Base::Camera camera(
|
||||
|
|
|
|||
|
|
@ -87,22 +87,28 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
|
|||
/* ************************************************************************* */
|
||||
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
|
||||
using namespace vanillaPoseRS;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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;
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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);
|
||||
}
|
||||
|
||||
|
|
@ -128,8 +134,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
|
|||
|
||||
FastVector<size_t> cameraIds{0, 0, 0};
|
||||
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(body_P_sensor, sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(body_P_sensor, sharedK));
|
||||
|
||||
// create by adding a batch of measurements with a bunch of calibrations
|
||||
SmartFactorRS::shared_ptr factor2(
|
||||
|
|
@ -183,8 +189,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Equals) {
|
|||
}
|
||||
{ // create slightly different factors (different extrinsics) and show equal
|
||||
// returns false
|
||||
Cameras cameraRig2;
|
||||
cameraRig2.push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig2(new Cameras());
|
||||
cameraRig2->push_back(Camera(body_P_sensor * body_P_sensor, sharedK));
|
||||
SmartFactorRS::shared_ptr factor1(
|
||||
new SmartFactorRS(model, cameraRig2, params));
|
||||
factor1->add(measurement1, x1, x2, interp_factor1, cameraId1);
|
||||
|
|
@ -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);
|
||||
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_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);
|
||||
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_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);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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);
|
||||
|
|
@ -463,9 +478,9 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_multiCam) {
|
|||
interp_factors.push_back(interp_factor2);
|
||||
interp_factors.push_back(interp_factor3);
|
||||
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(body_P_sensor, sharedK));
|
||||
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(body_P_sensor, sharedK));
|
||||
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
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_factor3);
|
||||
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(body_T_sensor1, sharedK));
|
||||
cameraRig.push_back(Camera(body_T_sensor2, sharedK));
|
||||
cameraRig.push_back(Camera(body_T_sensor3, sharedK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(body_T_sensor1, sharedK));
|
||||
cameraRig->push_back(Camera(body_T_sensor2, sharedK));
|
||||
cameraRig->push_back(Camera(body_T_sensor3, sharedK));
|
||||
|
||||
SmartFactorRS::shared_ptr smartFactor1(
|
||||
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(cam2.project(landmark1));
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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);
|
||||
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);
|
||||
|
||||
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);
|
||||
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);
|
||||
|
||||
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);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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);
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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
|
||||
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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++) {
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
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
|
||||
|
|
@ -1391,8 +1430,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
|
|||
gtsam::ZERO_ON_DEGENERACY); // only config that works with RS factors
|
||||
params.setRankTolerance(0.1);
|
||||
|
||||
Cameras cameraRig;
|
||||
cameraRig.push_back(Camera(Pose3::identity(), emptyK));
|
||||
boost::shared_ptr<Cameras> cameraRig(new Cameras());
|
||||
cameraRig->push_back(Camera(Pose3::identity(), emptyK));
|
||||
|
||||
SmartFactorRS_spherical::shared_ptr smartFactor1(
|
||||
new SmartFactorRS_spherical(model, cameraRig, params));
|
||||
|
|
|
|||
Loading…
Reference in New Issue