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

21
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -63,7 +63,7 @@ class SmartProjectionRigFactor : public SmartProjectionFactor<CAMERA> {
KeyVector nonUniqueKeys_;
/// 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),
/// identifying which camera took the measurement
@ -90,7 +90,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
@ -151,7 +152,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 "
@ -168,7 +169,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_; }
@ -186,7 +187,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);
}
@ -195,7 +196,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());
}
@ -210,7 +211,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
@ -249,7 +250,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);
@ -354,9 +355,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

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

@ -70,8 +70,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));
}
@ -79,7 +79,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
@ -90,8 +90,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);
@ -100,8 +100,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
@ -113,8 +113,8 @@ TEST(SmartProjectionRigFactor, Constructor4) {
/* ************************************************************************* */
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));
@ -141,8 +141,8 @@ TEST(SmartProjectionRigFactor, noiseless) {
Point2 level_uv = level_camera.project(landmark1);
Point2 level_uv_right = level_camera_right.project(landmark1);
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 factor(model, cameraRig, params);
factor.add(level_uv, x1); // both taken from the same camera
@ -198,8 +198,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);
@ -242,8 +242,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();
@ -327,10 +327,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();
@ -408,8 +408,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);
@ -496,8 +496,8 @@ TEST(SmartProjectionRigFactor, Factors) {
KeyVector views{x1, x2};
FastVector<size_t> cameraIds{0, 0};
Cameras cameraRig; // single camera in the rig
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
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, cameraRig, params);
@ -579,8 +579,8 @@ TEST(SmartProjectionRigFactor, 3poses_iterative_smart_projection_factor) {
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
// 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));
@ -647,8 +647,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(
@ -718,8 +718,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(
@ -784,8 +784,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(
@ -860,8 +860,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(
@ -890,8 +890,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);
@ -918,8 +918,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(
@ -989,8 +989,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(
@ -1117,8 +1117,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};
@ -1204,8 +1204,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);