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

moved rig to use shared ptrs

release/4.3a0
lcarlone 2021-11-07 18:25:35 -05:00
parent ac5875671f
commit 4ba93738ed
2 changed files with 54 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -63,7 +63,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
KeyVector nonUniqueKeys_; KeyVector nonUniqueKeys_;
/// cameras in the rig (fixed poses wrt body and intrinsics, for each camera) /// cameras in the rig (fixed poses wrt body and intrinsics, for each camera)
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
@ -90,7 +90,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
@ -151,7 +152,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 "
@ -168,7 +169,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_; }
@ -186,7 +187,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);
} }
@ -195,7 +196,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());
} }
@ -210,7 +211,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
@ -249,7 +250,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);

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

@ -70,8 +70,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));
} }
@ -79,7 +79,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
@ -90,8 +90,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);
@ -100,8 +100,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
@ -113,8 +113,8 @@ TEST(SmartProjectionRigFactor, Constructor4) {
/* ************************************************************************* */ /* ************************************************************************* */
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));
@ -141,8 +141,8 @@ 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);
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 factor(model, cameraRig, params); 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
@ -198,8 +198,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);
@ -242,8 +242,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();
@ -327,10 +327,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();
@ -408,8 +408,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);
@ -496,8 +496,8 @@ TEST(SmartProjectionRigFactor, Factors) {
KeyVector views{x1, x2}; KeyVector views{x1, x2};
FastVector<size_t> cameraIds{0, 0}; FastVector<size_t> cameraIds{0, 0};
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 smartFactor1 = boost::make_shared<SmartRigFactor>( SmartRigFactor::shared_ptr smartFactor1 = boost::make_shared<SmartRigFactor>(
model, cameraRig, params); model, cameraRig, params);
@ -579,8 +579,8 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// 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));
@ -647,8 +647,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(
@ -718,8 +718,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(
@ -784,8 +784,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(
@ -860,8 +860,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(
@ -890,8 +890,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);
@ -918,8 +918,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(
@ -989,8 +989,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(
@ -1117,8 +1117,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};
@ -1204,8 +1204,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);