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

now using shared ptrs

release/4.3a0
lcarlone 2021-11-07 18:32:43 -05:00
parent 4ba93738ed
commit 620f9cb99f
4 changed files with 51 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/slam/SmartProjectionRigFactor.h
View File

@ -196,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());
}

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

@ -29,7 +29,7 @@
#include <iostream>
#include "smartFactorScenarios.h"
//#define DISABLE_TIMING
#define DISABLE_TIMING
using namespace boost::assign;
using namespace std::placeholders;

17
gtsam_unstable/slam/SmartProjectionPoseFactorRollingShutter.h
View File

@ -59,7 +59,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
@ -95,7 +95,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
@ -175,7 +176,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 "
@ -200,7 +201,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_; }
@ -222,7 +223,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);
}
@ -251,7 +252,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());
}
@ -273,7 +274,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,
@ -326,7 +327,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);
PinholeCamera<CALIBRATION> camera(w_P_cam, camera_i.calibration());

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

@ -29,7 +29,7 @@
#include <iostream>
#include "gtsam/slam/tests/smartFactorScenarios.h"
//#define DISABLE_TIMING
#define DISABLE_TIMING
using namespace gtsam;
using namespace boost::assign;
@ -87,8 +87,8 @@ typedef SmartProjectionPoseFactorRollingShutter<PinholePose<Cal3_S2>>
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
using namespace vanillaPoseRS;
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params));
}
@ -96,8 +96,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor) {
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
using namespace vanillaPoseRS;
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
params.setRankTolerance(rankTol);
SmartFactorRS factor1(model, cameraRig, params);
}
@ -105,8 +105,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, Constructor2) {
/* ************************************************************************* */
TEST(SmartProjectionPoseFactorRollingShutter, add) {
using namespace vanillaPoseRS;
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr factor1(
new SmartFactorRS(model, cameraRig, params));
factor1->add(measurement1, x1, x2, interp_factor);
@ -134,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(
@ -189,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);
@ -232,8 +232,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, noiselessErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorId = Pose3::identity();
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorId, sharedK));
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);
@ -310,8 +310,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, noisyErrorAndJacobians) {
Point2 level_uv_right = cam2.project(landmark1);
Pose3 body_P_sensorNonId = body_P_sensor;
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorNonId, sharedK));
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);
@ -404,8 +404,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses) {
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
@ -478,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));
@ -566,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));
@ -648,8 +648,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, hessian_simple_2poses) {
measurements_lmk1.push_back(cam1.project(landmark1));
measurements_lmk1.push_back(cam2.project(landmark1));
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
@ -746,8 +746,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, optimization_3poses_EPI) {
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(true);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
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);
@ -815,8 +815,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist);
params.setEnableEPI(false);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
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);
@ -893,8 +893,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold);
params.setEnableEPI(false);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
@ -960,8 +960,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor2);
interp_factors.push_back(interp_factor3);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
@ -1101,8 +1101,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors.push_back(interp_factor3);
interp_factors.push_back(interp_factor1);
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
@ -1260,8 +1260,8 @@ TEST(SmartProjectionPoseFactorRollingShutter,
interp_factors_redundant.push_back(
interp_factors.at(0)); // we readd the first interp factor
Cameras cameraRig;
cameraRig.push_back(Camera(Pose3::identity(), sharedK));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(Pose3::identity(), sharedK));
SmartFactorRS::shared_ptr smartFactor1(
new SmartFactorRS(model, cameraRig, params));
@ -1349,8 +1349,8 @@ TEST(SmartProjectionPoseFactorRollingShutter, timing) {
size_t nrTests = 10000;
for (size_t i = 0; i < nrTests; i++) {
Cameras cameraRig;
cameraRig.push_back(Camera(body_P_sensorId, sharedKSimple));
boost::shared_ptr<Cameras> cameraRig(new Cameras());
cameraRig->push_back(Camera(body_P_sensorId, sharedKSimple));
SmartFactorRS::shared_ptr smartFactorRS(new SmartFactorRS(
model, cameraRig, params));