From 5677bdb6c12859af8552d78fa498ac4437a2e94c Mon Sep 17 00:00:00 2001 From: lcarlone Date: Mon, 29 Mar 2021 22:58:29 -0400 Subject: [PATCH] need to clean up templates and remove 2 redundant lines --- .../slam/SmartStereoProjectionFactor.h | 22 ++++-------- .../slam/SmartStereoProjectionFactorPP.h | 36 +++++-------------- .../testSmartStereoProjectionFactorPP.cpp | 4 +-- 3 files changed, 16 insertions(+), 46 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactor.h b/gtsam_unstable/slam/SmartStereoProjectionFactor.h index 535364caa..52fd99356 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 { + boost::optional Fs = boost::none, + boost::optional E = boost::none) const override + { // 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,28 +460,18 @@ public: { if(Fs){ // delete influence of right point on jacobian Fs MatrixZD& Fi = Fs->at(i); - for(size_t ii=0; iirow(ZD * i + 1) = Matrix::Zero(1, E->cols()); + E->row(ZDim * i + 1) = Matrix::Zero(1, E->cols()); // set the corresponding entry of vector ue to zero - ue(ZD * i + 1) = 0.0; + ue(ZDim * 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_; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 8aa789688..13a8a90f0 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -152,7 +152,7 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { /// Compute F, E only (called below in both vanilla and SVD versions) /// Assumes the point has been computed /// Note E can be 2m*3 or 2m*2, in case point is degenerate - void computeJacobiansWithTriangulatedPoint( + void computeJacobiansAndCorrectForMissingMeasurements( FBlocks& Fs, Matrix& E, Vector& b, const Values& values) const { if (!result_) { @@ -168,23 +168,20 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { Pose3 body_P_cam = values.at(body_P_cam_keys_.at(i)); StereoCamera camera(w_P_body.compose(body_P_cam, dPoseCam_dPoseBody, dPoseCam_dPoseExt), K_all_[i]); StereoPoint2 reprojectionError = StereoPoint2(camera.project(*result_, dProject_dPoseCam, Ei) - measured_.at(i)); -// std::cout << "H0 \n" << dPoseCam_dPoseBody << std::endl; -// std::cout << "H1 \n" << dProject_dPoseCam << std::endl; -// std::cout << "H3 \n" << Ei << std::endl; -// std::cout << "H02 \n" << dPoseCam_dPoseExt << std::endl; Eigen::Matrix J; // 3 x 12 -// std::cout << "H1 * H0 \n" << dProject_dPoseCam * dPoseCam_dPoseBody << std::endl; -// std::cout << "H1 * H02 \n" << dProject_dPoseCam * dPoseCam_dPoseExt << std::endl; J.block(0,0) = dProject_dPoseCam * dPoseCam_dPoseBody; // (3x6) * (6x6) J.block(0,6) = dProject_dPoseCam * dPoseCam_dPoseExt; // (3x6) * (6x6) -// std::cout << "J \n" << J << std::endl; + if(std::isnan(measured_.at(i).uR())) // if the right pixel is invalid + { + J.block<1,12>(1,0) = Matrix::Zero(1,12); + Ei.block<1,3>(1,0) = Matrix::Zero(1,3); + reprojectionError = StereoPoint2(reprojectionError.uL(), 0.0, reprojectionError.v() ); + } Fs.push_back(J); size_t row = 3*i; b.segment(row) = - reprojectionError.vector(); E.block<3,3>(row,0) = Ei; } - // correct for monocular measurements, where the right pixel measurement is nan - //Base::CorrectForMissingMeasurements(measured_, cameras, b, Fs, E); } } @@ -204,11 +201,9 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { throw std::runtime_error("SmartStereoProjectionHessianFactor: this->" "measured_.size() inconsistent with input"); - std::cout << "triangulate" << std::endl; triangulateSafe(cameras(values)); if (!result_) { - std::cout << "degenerate" << std::endl; // failed: return"empty" Hessian for(Matrix& m: Gs) m = Matrix::Zero(DimPose,DimPose); @@ -218,12 +213,11 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { Gs, gs, 0.0); } - std::cout << "params_.degeneracyMode" << params_.degeneracyMode << std::endl; // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). FBlocks Fs; Matrix F, E; Vector b; - computeJacobiansWithTriangulatedPoint(Fs, E, b, values); + computeJacobiansAndCorrectForMissingMeasurements(Fs, E, b, values); // Whiten using noise model noiseModel_->WhitenSystem(E, b); @@ -266,16 +260,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { keyToSlotMap[keys_[k]] = k; } - std::cout << "linearize" << std::endl; -// for(size_t i=0; i >(keys_, augmentedHessianUniqueKeys); - //std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) <(body_P_cam_key))); + EXPECT(assert_equal(sensor_to_body,result.at(body_P_cam_key), 1e-1)); } /* *************************************************************************/