diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 5010de8fd..535364caa 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactor.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactor.h @@ -449,10 +449,10 @@ public: /** * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) */ + template // D: camera dim, ZD: measurement dim void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, boost::optional Fs = boost::none, - boost::optional E = boost::none) const override - { + boost::optional E = boost::none) const { // when using stereo cameras, some of the measurements might be missing: for(size_t i=0; i < cameras.size(); i++){ const StereoPoint2& z = measured_.at(i); @@ -460,18 +460,28 @@ public: { if(Fs){ // delete influence of right point on jacobian Fs MatrixZD& Fi = Fs->at(i); - for(size_t ii=0; iirow(ZDim * i + 1) = Matrix::Zero(1, E->cols()); + E->row(ZD * i + 1) = Matrix::Zero(1, E->cols()); // set the corresponding entry of vector ue to zero - ue(ZDim * i + 1) = 0.0; + ue(ZD * i + 1) = 0.0; } } } + /** + * This corrects the Jacobians and error vector for the case in which the right pixel in the monocular camera is missing (nan) + * This is class implementation that directly uses the measurement and camera size without templates. + */ + void correctForMissingMeasurements(const Cameras& cameras, Vector& ue, + boost::optional Fs = boost::none, + boost::optional E = boost::none) const override { + correctForMissingMeasurements(cameras, ue, Fs, E); + } + /** return the landmark */ TriangulationResult point() const { return result_;