diff --git a/gtsam/slam/SmartFactorBase.h b/gtsam/slam/SmartFactorBase.h index e66b6ca6a..a247832e2 100644 --- a/gtsam/slam/SmartFactorBase.h +++ b/gtsam/slam/SmartFactorBase.h @@ -97,17 +97,32 @@ public: virtual ~SmartFactorBase() { } + // check that noise model is isotropic and the same + void maybeSetNoiseModel(const SharedNoiseModel& sharedNoiseModel) { + if (!sharedNoiseModel) + return; + SharedIsotropic sharedIsotropic = boost::dynamic_pointer_cast< + noiseModel::Isotropic>(sharedNoiseModel); + if (!sharedIsotropic) + throw std::runtime_error("SmartFactorBase: needs isotropic"); + if (!noiseModel_) + noiseModel_ = sharedIsotropic; + else if (!sharedIsotropic->equals(*noiseModel_)) + throw std::runtime_error( + "SmartFactorBase: cannot add measurements with different noise model"); + } + /** * Add a new measurement and pose key * @param measured_i is the 2m dimensional projection of a single landmark * @param poseKey is the index corresponding to the camera observing the landmark - * @param noise_i is the measurement noise + * @param sharedNoiseModel is the measurement noise */ void add(const Z& measured_i, const Key& poseKey_i, - const SharedNoiseModel& noise_i) { + const SharedNoiseModel& sharedNoiseModel) { this->measured_.push_back(measured_i); this->keys_.push_back(poseKey_i); - this->noise_.push_back(noise_i); + maybeSetNoiseModel(sharedNoiseModel); } /** @@ -118,7 +133,7 @@ public: for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noises.at(i)); + maybeSetNoiseModel(noises.at(i)); } } @@ -130,7 +145,7 @@ public: for (size_t i = 0; i < measurements.size(); i++) { this->measured_.push_back(measurements.at(i)); this->keys_.push_back(poseKeys.at(i)); - this->noise_.push_back(noise); + maybeSetNoiseModel(noise); } } @@ -143,7 +158,7 @@ public: for (size_t k = 0; k < trackToAdd.number_measurements(); k++) { this->measured_.push_back(trackToAdd.measurements[k].second); this->keys_.push_back(trackToAdd.measurements[k].first); - this->noise_.push_back(noise); + maybeSetNoiseModel(noise); } } @@ -223,7 +238,7 @@ public: Vector b = reprojectionError(cameras, point); size_t nrCameras = cameras.size(); for (size_t i = 0, row = 0; i < nrCameras; i++, row += ZDim) - b.segment(row) = noise_.at(i)->whiten(b.segment(row)); + b.segment(row) = noiseModel_->whiten(b.segment(row)); return b; } @@ -240,7 +255,7 @@ public: double overallError = 0; size_t nrCameras = cameras.size(); for (size_t i = 0; i < nrCameras; i++) - overallError += noise_.at(i)->distance(b.segment(i * ZDim)); + overallError += noiseModel_->distance(b.segment(i * ZDim)); return 0.5 * overallError; } @@ -272,7 +287,7 @@ public: Vector bi = (zi - predicted[i]).vector(); // if needed, whiten - SharedNoiseModel model = noise_.at(i); + SharedNoiseModel model = noiseModel_; if (model) { // TODO: re-factor noiseModel to take any block/fixed vector/matrix Vector dummy; @@ -325,16 +340,15 @@ public: } // if needed, whiten - SharedNoiseModel model = noise_.at(i); - if (model) { + if (noiseModel_) { // TODO, refactor noiseModel so we can take blocks Matrix Fi = F.block(row, 0); Matrix Ei = E.block(row, 0); if (!G) - model->WhitenSystem(Fi, Ei, bi); + noiseModel_->WhitenSystem(Fi, Ei, bi); else { Matrix Gi = G->block(row, 0); - model->WhitenSystem(Fi, Ei, Gi, bi); + noiseModel_->WhitenSystem(Fi, Ei, Gi, bi); G->block(row, 0) = Gi; } F.block(row, 0) = Fi; @@ -413,7 +427,8 @@ public: } /// Create BIG block-diagonal matrix F from Fblocks - static void FillDiagonalF(const std::vector& Fblocks, Matrix& F) { + static void FillDiagonalF(const std::vector& Fblocks, + Matrix& F) { size_t m = Fblocks.size(); F.resize(ZDim * m, D * m); F.setZero(); diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 7fb43152a..ca49272ec 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -533,7 +533,7 @@ public: Vector bi = -(cameras[i].projectPointAtInfinity(this->point_, Fi, Ei) - this->measured_.at(i)).vector(); - this->noise_.at(i)->WhitenSystem(Fi, Ei, bi); + this->noiseModel_->WhitenSystem(Fi, Ei, bi); f += bi.squaredNorm(); Fblocks.push_back(typename Base::KeyMatrix2D(this->keys_[i], Fi)); E.block<2, 2>(2 * i, 0) = Ei; @@ -639,7 +639,7 @@ public: Point2 reprojectionError( camera.projectPointAtInfinity(this->point_) - zi); overallError += 0.5 - * this->noise_.at(i)->distance(reprojectionError.vector()); + * this->noiseModel_->distance(reprojectionError.vector()); i += 1; } return overallError;