From 7c052ff48a6852e30d833800190bedfde1d883ed Mon Sep 17 00:00:00 2001 From: lcarlone Date: Thu, 25 Mar 2021 21:37:13 -0400 Subject: [PATCH] fixed print, removed cout, test still failing --- .../slam/SmartStereoProjectionFactorPP.cpp | 2 +- .../slam/SmartStereoProjectionFactorPP.h | 53 +++++++++---------- .../testSmartStereoProjectionFactorPP.cpp | 28 ++++++---- 3 files changed, 43 insertions(+), 40 deletions(-) diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp index 9012522b3..3584c7683 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.cpp @@ -76,7 +76,7 @@ void SmartStereoProjectionFactorPP::add( void SmartStereoProjectionFactorPP::print( const std::string& s, const KeyFormatter& keyFormatter) const { - std::cout << s << "SmartStereoProjectionFactorPP, z = \n "; + std::cout << s << "SmartStereoProjectionFactorPP: \n "; for (size_t i = 0; i < K_all_.size(); i++) { K_all_[i]->print("calibration = "); std::cout << " extrinsic pose key: " << keyFormatter(body_P_cam_keys_[i]) << std::endl; diff --git a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h index 06edd6a56..6db6b6dfa 100644 --- a/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h +++ b/gtsam_unstable/slam/SmartStereoProjectionFactorPP.h @@ -169,16 +169,16 @@ 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; +// 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; +// 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; +// std::cout << "J \n" << J << std::endl; Fs.push_back(J); size_t row = 3*i; b.segment(row) = - reprojectionError.vector(); @@ -205,7 +205,6 @@ class SmartStereoProjectionFactorPP : public SmartStereoProjectionFactor { std::vector Gs(numKeys * (numKeys + 1) / 2); std::vector gs(numKeys); - std::cout <<"using my hessian!!!!!!!!!!1" << std::endl; for(size_t i=0; i >(allKeys, Gs, gs, 0.0); } - +// std::cout << "result_" << *result_ << std::endl; +// std::cout << "result_2" << result_ << std::endl; // Jacobian could be 3D Point3 OR 2D Unit3, difference is E.cols(). FBlocks Fs; Matrix F, E; Vector b; - std::cout <<"before computeJacobiansWithTriangulatedPoint!!!!!!!!!!1" << std::endl; computeJacobiansWithTriangulatedPoint(Fs, E, b, values); - std::cout << "Fs.at(0) \n"<< Fs.at(0) << std::endl; - std::cout << "Dim "<< Dim << std::endl; - std::cout << "numKeys "<< numKeys << std::endl; - - std::cout << "Fs.size() = " << Fs.size() << std::endl; - std::cout << "E = " << E << std::endl; - std::cout << "b = " << b << std::endl; +// std::cout << "Dim "<< Dim << std::endl; +// std::cout << "numKeys "<< numKeys << std::endl; +// +// std::cout << "Fs.size() = " << Fs.size() << std::endl; +// std::cout << "E = " << E << std::endl; +// std::cout << "b = " << b << std::endl; // Whiten using noise model - std::cout << "noise model1 \n " << std::endl; +// std::cout << "noise model1 \n " << std::endl; noiseModel_->WhitenSystem(E, b); - std::cout << "noise model2 \n " << std::endl; +// std::cout << "noise model2 \n " << std::endl; for (size_t i = 0; i < Fs.size(); i++) Fs[i] = noiseModel_->Whiten(Fs[i]); - std::cout << "noise model3 \n " << std::endl; +// std::cout << "noise model3 \n " << std::endl; // build augmented hessian Matrix3 P; Cameras::ComputePointCovariance<3>(P, E, lambda, diagonalDamping); - std::cout << "ComputePointCovariance done!!! \n " << std::endl; - std::cout << "Fs.size() = " << Fs.size() << std::endl; - std::cout << "E = " << E << std::endl; - std::cout << "P = " << P << std::endl; - std::cout << "b = " << b << std::endl; +// std::cout << "ComputePointCovariance done!!! \n " << std::endl; +// std::cout << "Fs.size() = " << Fs.size() << std::endl; +// std::cout << "E = " << E << std::endl; +// std::cout << "P = " << P << std::endl; +// std::cout << "b = " << b << std::endl; SymmetricBlockMatrix augmentedHessian = // Cameras::SchurComplement<3,Dim>(Fs, E, P, b); - std::cout << "Repackaging!!! \n " << std::endl; - std::vector dims(numKeys + 1); // this also includes the b term std::fill(dims.begin(), dims.end() - 1, 6); dims.back() = 1; SymmetricBlockMatrix augmentedHessianPP(dims, Matrix(augmentedHessian.selfadjointView())); + std::cout << "Matrix(augmentedHessian.selfadjointView()) \n" << Matrix(augmentedHessian.selfadjointView()) < >(allKeys, augmentedHessianPP); diff --git a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp index 7a22f6f17..21c7c8b79 100644 --- a/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp +++ b/gtsam_unstable/slam/tests/testSmartStereoProjectionFactorPP.cpp @@ -659,22 +659,14 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization SmartStereoProjectionFactorPP::shared_ptr smartFactor3(new SmartStereoProjectionFactorPP(model, smart_params)); smartFactor3->add(measurements_l3, poseKeys, extrinsicKeys, K2); - const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); - - // Values + // relevant poses: Pose3 body_Pose_cam = Pose3(Rot3::Ypr(-M_PI, 1., 0.1),Point3(0, 1, 0)); Pose3 w_Pose_body1 = w_Pose_cam1.compose(body_Pose_cam.inverse()); Pose3 w_Pose_body2 = w_Pose_cam2.compose(body_Pose_cam.inverse()); Pose3 w_Pose_body3 = w_Pose_cam3.compose(body_Pose_cam.inverse()); - Values values; - Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise - values.insert(x1, w_Pose_body1); - values.insert(x2, w_Pose_body2); - values.insert(x3, w_Pose_body3); - values.insert(body_P_cam_key, body_Pose_cam*noise_pose); - // Graph + const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); NonlinearFactorGraph graph; graph.push_back(smartFactor1); graph.push_back(smartFactor2); @@ -683,7 +675,15 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization graph.addPrior(x2, w_Pose_body2, noisePrior); graph.addPrior(x3, w_Pose_body3, noisePrior); // we might need some prior on the calibration too - // graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this! + graph.addPrior(body_P_cam_key, body_Pose_cam, noisePrior); // no need! the measurements will fix this! + + // Values + Values values; + Pose3 noise_pose = Pose3(Rot3::Ypr(-M_PI / 100, 0., -M_PI / 100), Point3(0.01, 0.01, 0.01)); // smaller noise + values.insert(x1, w_Pose_body1); + values.insert(x2, w_Pose_body2); + values.insert(x3, w_Pose_body3); + values.insert(body_P_cam_key, body_Pose_cam*noise_pose); // cost is large before optimization double initialErrorSmart = graph.error(values); @@ -697,6 +697,12 @@ TEST( SmartStereoProjectionFactorPP, 3poses_smart_projection_factor_optimization tictoc_finishedIteration_(); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-5); + + GaussianFactorGraph::shared_ptr GFG = graph.linearize(result); + GFG->print("GFG \n"); + VectorValues delta = GFG->optimize(); + VectorValues expected = VectorValues::Zero(delta); + EXPECT(assert_equal(expected, delta, 1e-6)); } /* *************************************************************************