diff --git a/demos/mcapi_pov_estimation.cpp b/demos/mcapi_pov_estimation.cpp index 7199e8aedad634ab42c37d16cd30d54c4a12a6fc..2bbb1da3f429a6c56fe2af14ad36484f55bf3b6f 100644 --- a/demos/mcapi_pov_estimation.cpp +++ b/demos/mcapi_pov_estimation.cpp @@ -646,66 +646,62 @@ int main (int argc, char **argv) { VectorComposite bi_bias_est; for (auto& elt: problem->getTrajectory()->getFrameMap()){ auto kf = elt.second; - if (kf->isKey()){ - kf_state = kf->getState(); - cap_imu = kf->getCaptureOf(sen_imu); - bi_bias_est = cap_imu->getState(); - - file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) - << kf->getTimeStamp().get() << "," - << kf_state['P'](0) << "," - << kf_state['P'](1) << "," - << kf_state['P'](2) << "," - << kf_state['O'](0) << "," - << kf_state['O'](1) << "," - << kf_state['O'](2) << "," - << kf_state['O'](3) << "," - << kf_state['V'](0) << "," - << kf_state['V'](1) << "," - << kf_state['V'](2) << "," - << bi_bias_est['I'](0) << "," - << bi_bias_est['I'](1) << "," - << bi_bias_est['I'](2) << "," - << bi_bias_est['I'](3) << "," - << bi_bias_est['I'](4) << "," - << bi_bias_est['I'](5) - << "\n"; - - // compute covariances of KF and capture stateblocks - Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local? - Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity(); - - solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below - problem->getCovarianceBlock(kf->getStateBlock('P'), Qp); - problem->getCovarianceBlock(kf->getStateBlock('O'), Qo); - problem->getCovarianceBlock(kf->getStateBlock('V'), Qv); - - solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise - problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi); - - file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1) - << kf->getTimeStamp().get() << "," - << Qp(0,0) << "," - << Qp(1,1) << "," - << Qp(2,2) << "," - << Qo(0,0) << "," - << Qo(1,1) << "," - << Qo(2,2) << "," - << Qv(0,0) << "," - << Qv(1,1) << "," - << Qv(2,2) << "," - << Qbi(0,0) << "," - << Qbi(1,1) << "," - << Qbi(2,2) << "," - << Qbi(3,3) << "," - << Qbi(4,4) << "," - << Qbi(5,5) - << "\n"; - - - } + kf_state = kf->getState(); + cap_imu = kf->getCaptureOf(sen_imu); + bi_bias_est = cap_imu->getState(); + + file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) + << kf->getTimeStamp().get() << "," + << kf_state['P'](0) << "," + << kf_state['P'](1) << "," + << kf_state['P'](2) << "," + << kf_state['O'](0) << "," + << kf_state['O'](1) << "," + << kf_state['O'](2) << "," + << kf_state['O'](3) << "," + << kf_state['V'](0) << "," + << kf_state['V'](1) << "," + << kf_state['V'](2) << "," + << bi_bias_est['I'](0) << "," + << bi_bias_est['I'](1) << "," + << bi_bias_est['I'](2) << "," + << bi_bias_est['I'](3) << "," + << bi_bias_est['I'](4) << "," + << bi_bias_est['I'](5) + << "\n"; + + // compute covariances of KF and capture stateblocks + Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local? + Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity(); + + solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below + problem->getCovarianceBlock(kf->getStateBlock('P'), Qp); + problem->getCovarianceBlock(kf->getStateBlock('O'), Qo); + problem->getCovarianceBlock(kf->getStateBlock('V'), Qv); + + solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise + problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi); + + file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1) + << kf->getTimeStamp().get() << "," + << Qp(0,0) << "," + << Qp(1,1) << "," + << Qp(2,2) << "," + << Qo(0,0) << "," + << Qo(1,1) << "," + << Qo(2,2) << "," + << Qv(0,0) << "," + << Qv(1,1) << "," + << Qv(2,2) << "," + << Qbi(0,0) << "," + << Qbi(1,1) << "," + << Qbi(2,2) << "," + << Qbi(3,3) << "," + << Qbi(4,4) << "," + << Qbi(5,5) + << "\n"; } file_gtr.close(); diff --git a/demos/mcapi_povcdl_estimation.cpp b/demos/mcapi_povcdl_estimation.cpp index 38206e6ba37667f515d51fcd02ace7a85f585395..6a1e908bff0d6abffdb7ae0b2e7a5edd994af928 100644 --- a/demos/mcapi_povcdl_estimation.cpp +++ b/demos/mcapi_povcdl_estimation.cpp @@ -474,7 +474,6 @@ int main (int argc, char **argv) { params_sen_ft->dist_traveled = 20000.0; params_sen_ft->angle_turned = 1000; params_sen_ft->voting_active = false; - params_sen_ft->voting_aux_active = false; ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft); // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint"); ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base); @@ -501,7 +500,7 @@ int main (int argc, char **argv) { P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity(); P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity(); P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity(); - FrameBasePtr KF1 = problem->emplaceKeyFrame(t0, x_origin); + FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin); // Prior pose factor CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6)); pose_prior_capture->emplaceFeatureAndFactor(); @@ -845,103 +844,99 @@ int main (int argc, char **argv) { VectorComposite bi_bias_est; for (auto& elt: problem->getTrajectory()->getFrameMap()){ auto kf = elt.second; - if (kf->isKey()){ - kf_state = kf->getState(); - cap_ikin = kf->getCaptureOf(sen_ikin); - bp_bias_est = cap_ikin->getState(); - cap_imu = kf->getCaptureOf(sen_imu); - bi_bias_est = cap_imu->getState(); - - file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) - << kf->getTimeStamp().get() << "," - << kf_state['P'](0) << "," - << kf_state['P'](1) << "," - << kf_state['P'](2) << "," - << kf_state['O'](0) << "," - << kf_state['O'](1) << "," - << kf_state['O'](2) << "," - << kf_state['O'](3) << "," - << kf_state['V'](0) << "," - << kf_state['V'](1) << "," - << kf_state['V'](2) << "," - << bi_bias_est['I'](0) << "," - << bi_bias_est['I'](1) << "," - << bi_bias_est['I'](2) << "," - << bi_bias_est['I'](3) << "," - << bi_bias_est['I'](4) << "," - << bi_bias_est['I'](5) << "," - << kf_state['C'](0) << "," - << kf_state['C'](1) << "," - << kf_state['C'](2) << "," - << kf_state['D'](0) << "," - << kf_state['D'](1) << "," - << kf_state['D'](2) << "," - << kf_state['L'](0) << "," - << kf_state['L'](1) << "," - << kf_state['L'](2) << "," - << bp_bias_est['I'](0) << "," - << bp_bias_est['I'](1) << "," - << bp_bias_est['I'](2) - << "\n"; - - // compute covariances of KF and capture stateblocks - Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local? - Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity(); - Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd QL = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity(); - // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended... - - solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below - problem->getCovarianceBlock(kf->getStateBlock('P'), Qp); - problem->getCovarianceBlock(kf->getStateBlock('O'), Qo); - problem->getCovarianceBlock(kf->getStateBlock('V'), Qv); - problem->getCovarianceBlock(kf->getStateBlock('C'), Qc); - problem->getCovarianceBlock(kf->getStateBlock('D'), Qd); - problem->getCovarianceBlock(kf->getStateBlock('L'), QL); - - solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise - problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp); - // std::cout << "Qbp\n" << Qbp << std::endl; - solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise - problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi); - - file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1) - << kf->getTimeStamp().get() << "," - << Qp(0,0) << "," - << Qp(1,1) << "," - << Qp(2,2) << "," - << Qo(0,0) << "," - << Qo(1,1) << "," - << Qo(2,2) << "," - << Qv(0,0) << "," - << Qv(1,1) << "," - << Qv(2,2) << "," - << Qbi(0,0) << "," - << Qbi(1,1) << "," - << Qbi(2,2) << "," - << Qbi(3,3) << "," - << Qbi(4,4) << "," - << Qbi(5,5) << "," - << Qc(0,0) << "," - << Qc(1,1) << "," - << Qc(2,2) << "," - << Qd(0,0) << "," - << Qd(1,1) << "," - << Qd(2,2) << "," - << QL(0,0) << "," - << QL(1,1) << "," - << QL(2,2) << "," - << Qbp(0,0) << "," - << Qbp(1,1) << "," - << Qbp(2,2) - << "\n"; - - - } + kf_state = kf->getState(); + cap_ikin = kf->getCaptureOf(sen_ikin); + bp_bias_est = cap_ikin->getState(); + cap_imu = kf->getCaptureOf(sen_imu); + bi_bias_est = cap_imu->getState(); + + file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) + << kf->getTimeStamp().get() << "," + << kf_state['P'](0) << "," + << kf_state['P'](1) << "," + << kf_state['P'](2) << "," + << kf_state['O'](0) << "," + << kf_state['O'](1) << "," + << kf_state['O'](2) << "," + << kf_state['O'](3) << "," + << kf_state['V'](0) << "," + << kf_state['V'](1) << "," + << kf_state['V'](2) << "," + << bi_bias_est['I'](0) << "," + << bi_bias_est['I'](1) << "," + << bi_bias_est['I'](2) << "," + << bi_bias_est['I'](3) << "," + << bi_bias_est['I'](4) << "," + << bi_bias_est['I'](5) << "," + << kf_state['C'](0) << "," + << kf_state['C'](1) << "," + << kf_state['C'](2) << "," + << kf_state['D'](0) << "," + << kf_state['D'](1) << "," + << kf_state['D'](2) << "," + << kf_state['L'](0) << "," + << kf_state['L'](1) << "," + << kf_state['L'](2) << "," + << bp_bias_est['I'](0) << "," + << bp_bias_est['I'](1) << "," + << bp_bias_est['I'](2) + << "\n"; + + // compute covariances of KF and capture stateblocks + Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local? + Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity(); + Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd QL = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity(); + // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended... + + solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below + problem->getCovarianceBlock(kf->getStateBlock('P'), Qp); + problem->getCovarianceBlock(kf->getStateBlock('O'), Qo); + problem->getCovarianceBlock(kf->getStateBlock('V'), Qv); + problem->getCovarianceBlock(kf->getStateBlock('C'), Qc); + problem->getCovarianceBlock(kf->getStateBlock('D'), Qd); + problem->getCovarianceBlock(kf->getStateBlock('L'), QL); + + solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise + problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp); + // std::cout << "Qbp\n" << Qbp << std::endl; + solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise + problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi); + + file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1) + << kf->getTimeStamp().get() << "," + << Qp(0,0) << "," + << Qp(1,1) << "," + << Qp(2,2) << "," + << Qo(0,0) << "," + << Qo(1,1) << "," + << Qo(2,2) << "," + << Qv(0,0) << "," + << Qv(1,1) << "," + << Qv(2,2) << "," + << Qbi(0,0) << "," + << Qbi(1,1) << "," + << Qbi(2,2) << "," + << Qbi(3,3) << "," + << Qbi(4,4) << "," + << Qbi(5,5) << "," + << Qc(0,0) << "," + << Qc(1,1) << "," + << Qc(2,2) << "," + << Qd(0,0) << "," + << Qd(1,1) << "," + << Qd(2,2) << "," + << QL(0,0) << "," + << QL(1,1) << "," + << QL(2,2) << "," + << Qbp(0,0) << "," + << Qbp(1,1) << "," + << Qbp(2,2) + << "\n"; } file_gtr.close(); diff --git a/demos/params_tree_manager.yaml b/demos/params_tree_manager.yaml index 632501863c81180b1d042ae5476855ff30308c54..6355024a0ba3875a0a5981422864e7828eb04f26 100644 --- a/demos/params_tree_manager.yaml +++ b/demos/params_tree_manager.yaml @@ -2,6 +2,6 @@ config: problem: tree_manager: fix_first_frame: true - n_frames: 10 + n_frames: 5 type: TreeManagerSlidingWindow viral_remove_empty_parent: true diff --git a/demos/processor_imu_solo12.yaml b/demos/processor_imu_solo12.yaml index 2092da805973a9bdce6752312bd31cd935a8ddc9..9ff74a3a27de6086489f87597829377cc7c6db36 100644 --- a/demos/processor_imu_solo12.yaml +++ b/demos/processor_imu_solo12.yaml @@ -8,5 +8,4 @@ keyframe_vote: max_buff_length: 100000000000 # motion deltas dist_traveled: 20000.0 # meters angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg) - voting_active: true - voting_aux_active: false \ No newline at end of file + voting_active: true \ No newline at end of file diff --git a/demos/processor_odom_3d.yaml b/demos/processor_odom_3d.yaml index d3633287a618fed2899767c6c99e5f548176d0f3..950b77442fd54510ac688dd8b4e1d884d6888d9d 100644 --- a/demos/processor_odom_3d.yaml +++ b/demos/processor_odom_3d.yaml @@ -5,7 +5,6 @@ unmeasured_perturbation_std: 0.0001 keyframe_vote: voting_active: false - voting_aux_active: false max_time_span: 100 # seconds max_buff_length: 10 # motion deltas dist_traveled: 0.5 # meters diff --git a/demos/solo_real_povcdl_estimation.cpp b/demos/solo_real_povcdl_estimation.cpp index 5ecd84218ab80f5682f4cf481297571d138163fa..0f0397f27b765e02e14f35d9a7c07342a50636b9 100644 --- a/demos/solo_real_povcdl_estimation.cpp +++ b/demos/solo_real_povcdl_estimation.cpp @@ -279,7 +279,6 @@ int main (int argc, char **argv) { params_sen_ft->dist_traveled = 20000.0; params_sen_ft->angle_turned = 1000; params_sen_ft->voting_active = false; - params_sen_ft->voting_aux_active = false; ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", sen_ft, params_sen_ft); // ProcessorBasePtr proc_ft_base = problem->installProcessor("ProcessorForceTorquePreint", "PFTPreint", "SenFT", bodydynamics_root_dir + "/demos/processor_ft_preint.yaml"); ProcessorForceTorquePreintPtr proc_ftpreint = std::static_pointer_cast<ProcessorForceTorquePreint>(proc_ft_base); @@ -313,7 +312,7 @@ int main (int argc, char **argv) { P_origin.block<3,3>(0,0) = pow(std_prior_p, 2) * Matrix3d::Identity(); P_origin.block<3,3>(3,3) = pow(std_prior_o, 2) * Matrix3d::Identity(); P_origin.block<3,3>(6,6) = pow(std_prior_v, 2) * Matrix3d::Identity(); - FrameBasePtr KF1 = problem->emplaceKeyFrame(t0, x_origin); + FrameBasePtr KF1 = problem->emplaceFrame(t0, x_origin); // Prior pose factor CapturePosePtr pose_prior_capture = CaptureBase::emplace<CapturePose>(KF1, t0, nullptr, x_origin.head(7), P_origin.topLeftCorner(6, 6)); pose_prior_capture->emplaceFeatureAndFactor(); @@ -723,103 +722,99 @@ int main (int argc, char **argv) { VectorComposite bi_bias_est; for (auto& elt: problem->getTrajectory()->getFrameMap()){ auto kf = elt.second; - if (kf->isKey()){ - kf_state = kf->getState(); - cap_ikin = kf->getCaptureOf(sen_ikin); - bp_bias_est = cap_ikin->getState(); - cap_imu = kf->getCaptureOf(sen_imu); - bi_bias_est = cap_imu->getState(); - - file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) - << kf->getTimeStamp().get() << "," - << kf_state['P'](0) << "," - << kf_state['P'](1) << "," - << kf_state['P'](2) << "," - << kf_state['O'](0) << "," - << kf_state['O'](1) << "," - << kf_state['O'](2) << "," - << kf_state['O'](3) << "," - << kf_state['V'](0) << "," - << kf_state['V'](1) << "," - << kf_state['V'](2) << "," - << bi_bias_est['I'](0) << "," - << bi_bias_est['I'](1) << "," - << bi_bias_est['I'](2) << "," - << bi_bias_est['I'](3) << "," - << bi_bias_est['I'](4) << "," - << bi_bias_est['I'](5) << "," - << kf_state['C'](0) << "," - << kf_state['C'](1) << "," - << kf_state['C'](2) << "," - << kf_state['D'](0) << "," - << kf_state['D'](1) << "," - << kf_state['D'](2) << "," - << kf_state['L'](0) << "," - << kf_state['L'](1) << "," - << kf_state['L'](2) << "," - << bp_bias_est['I'](0) << "," - << bp_bias_est['I'](1) << "," - << bp_bias_est['I'](2) - << "\n"; - - // compute covariances of KF and capture stateblocks - Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local? - Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity(); - Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd QL = Eigen::Matrix3d::Identity(); - Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity(); - // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended... - - solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below - problem->getCovarianceBlock(kf->getStateBlock('P'), Qp); - problem->getCovarianceBlock(kf->getStateBlock('O'), Qo); - problem->getCovarianceBlock(kf->getStateBlock('V'), Qv); - problem->getCovarianceBlock(kf->getStateBlock('C'), Qc); - problem->getCovarianceBlock(kf->getStateBlock('D'), Qd); - problem->getCovarianceBlock(kf->getStateBlock('L'), QL); - - solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise - problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp); - // std::cout << "Qbp\n" << Qbp << std::endl; - solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise - problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi); - - file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1) - << kf->getTimeStamp().get() << "," - << Qp(0,0) << "," - << Qp(1,1) << "," - << Qp(2,2) << "," - << Qo(0,0) << "," - << Qo(1,1) << "," - << Qo(2,2) << "," - << Qv(0,0) << "," - << Qv(1,1) << "," - << Qv(2,2) << "," - << Qbi(0,0) << "," - << Qbi(1,1) << "," - << Qbi(2,2) << "," - << Qbi(3,3) << "," - << Qbi(4,4) << "," - << Qbi(5,5) << "," - << Qc(0,0) << "," - << Qc(1,1) << "," - << Qc(2,2) << "," - << Qd(0,0) << "," - << Qd(1,1) << "," - << Qd(2,2) << "," - << QL(0,0) << "," - << QL(1,1) << "," - << QL(2,2) << "," - << Qbp(0,0) << "," - << Qbp(1,1) << "," - << Qbp(2,2) - << "\n"; - - - } + kf_state = kf->getState(); + cap_ikin = kf->getCaptureOf(sen_ikin); + bp_bias_est = cap_ikin->getState(); + cap_imu = kf->getCaptureOf(sen_imu); + bi_bias_est = cap_imu->getState(); + + file_kfs << std::setprecision(std::numeric_limits<long double>::digits10 + 1) + << kf->getTimeStamp().get() << "," + << kf_state['P'](0) << "," + << kf_state['P'](1) << "," + << kf_state['P'](2) << "," + << kf_state['O'](0) << "," + << kf_state['O'](1) << "," + << kf_state['O'](2) << "," + << kf_state['O'](3) << "," + << kf_state['V'](0) << "," + << kf_state['V'](1) << "," + << kf_state['V'](2) << "," + << bi_bias_est['I'](0) << "," + << bi_bias_est['I'](1) << "," + << bi_bias_est['I'](2) << "," + << bi_bias_est['I'](3) << "," + << bi_bias_est['I'](4) << "," + << bi_bias_est['I'](5) << "," + << kf_state['C'](0) << "," + << kf_state['C'](1) << "," + << kf_state['C'](2) << "," + << kf_state['D'](0) << "," + << kf_state['D'](1) << "," + << kf_state['D'](2) << "," + << kf_state['L'](0) << "," + << kf_state['L'](1) << "," + << kf_state['L'](2) << "," + << bp_bias_est['I'](0) << "," + << bp_bias_est['I'](1) << "," + << bp_bias_est['I'](2) + << "\n"; + + // compute covariances of KF and capture stateblocks + Eigen::MatrixXd Qp = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qo = Eigen::Matrix3d::Identity(); // global or local? + Eigen::MatrixXd Qv = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qbi = Eigen::Matrix6d::Identity(); + Eigen::MatrixXd Qc = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qd = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd QL = Eigen::Matrix3d::Identity(); + Eigen::MatrixXd Qbp = Eigen::Matrix3d::Identity(); + // solver->computeCovariances(kf->getStateBlockVec()); // !! does not work as intended... + + solver->computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL); // should not be computed every time !! see below + problem->getCovarianceBlock(kf->getStateBlock('P'), Qp); + problem->getCovarianceBlock(kf->getStateBlock('O'), Qo); + problem->getCovarianceBlock(kf->getStateBlock('V'), Qv); + problem->getCovarianceBlock(kf->getStateBlock('C'), Qc); + problem->getCovarianceBlock(kf->getStateBlock('D'), Qd); + problem->getCovarianceBlock(kf->getStateBlock('L'), QL); + + solver->computeCovariances(cap_ikin->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise + problem->getCovarianceBlock(cap_ikin->getSensorIntrinsic(), Qbp); + // std::cout << "Qbp\n" << Qbp << std::endl; + solver->computeCovariances(cap_imu->getStateBlockVec()); // not computed by ALL and destroys other computed covs -> issue to raise + problem->getCovarianceBlock(cap_imu->getSensorIntrinsic(), Qbi); + + file_cov << std::setprecision(std::numeric_limits<long double>::digits10 + 1) + << kf->getTimeStamp().get() << "," + << Qp(0,0) << "," + << Qp(1,1) << "," + << Qp(2,2) << "," + << Qo(0,0) << "," + << Qo(1,1) << "," + << Qo(2,2) << "," + << Qv(0,0) << "," + << Qv(1,1) << "," + << Qv(2,2) << "," + << Qbi(0,0) << "," + << Qbi(1,1) << "," + << Qbi(2,2) << "," + << Qbi(3,3) << "," + << Qbi(4,4) << "," + << Qbi(5,5) << "," + << Qc(0,0) << "," + << Qc(1,1) << "," + << Qc(2,2) << "," + << Qd(0,0) << "," + << Qd(1,1) << "," + << Qd(2,2) << "," + << QL(0,0) << "," + << QL(1,1) << "," + << QL(2,2) << "," + << Qbp(0,0) << "," + << Qbp(1,1) << "," + << Qbp(2,2) + << "\n"; } for (unsigned int i=0; i < N; i++) { diff --git a/test/gtest_processor_point_feet_nomove.cpp b/test/gtest_processor_point_feet_nomove.cpp index af69cf52af0f12ff73149729d7409b5665f2d2f3..46eac65e5d406bea477078112ebfd5e202488240 100644 --- a/test/gtest_processor_point_feet_nomove.cpp +++ b/test/gtest_processor_point_feet_nomove.cpp @@ -136,10 +136,10 @@ TEST_F(PointFeetCaptures, process_all_capture_first) CPF6_->process(); // one last capture to create the factor // factor creation due to keyFrameCallback - FrameBasePtr KF1 = problem_->emplaceKeyFrame(2.0, x_origin_); + FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_); proc_pfnm_base_->keyFrameCallback(KF1, 0.001); - FrameBasePtr KF2 = problem_->emplaceKeyFrame(3.0, x_origin_); + FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_); proc_pfnm_base_->keyFrameCallback(KF2, 0.001); problem_->print(4,1,1,1); @@ -149,10 +149,10 @@ TEST_F(PointFeetCaptures, process_all_capture_first) TEST_F(PointFeetCaptures, process_all_capture_last) { // First, get the key frames from the keyFrameCallback - FrameBasePtr KF1 = problem_->emplaceKeyFrame(2.0, x_origin_); + FrameBasePtr KF1 = problem_->emplaceFrame(2.0, x_origin_); proc_pfnm_base_->keyFrameCallback(KF1, 0.001); - FrameBasePtr KF2 = problem_->emplaceKeyFrame(3.0, x_origin_); + FrameBasePtr KF2 = problem_->emplaceFrame(3.0, x_origin_); proc_pfnm_base_->keyFrameCallback(KF2, 0.001); // Then process the captures diff --git a/test/processor_imu.yaml b/test/processor_imu.yaml index 9cf58bfe5574a81713cd2496dd3a48738a2aa7c9..468ed456e8fff451dc495eeb2e48b1eabaf104be 100644 --- a/test/processor_imu.yaml +++ b/test/processor_imu.yaml @@ -7,5 +7,4 @@ keyframe_vote: max_buff_length: 500 # motion deltas dist_traveled: 20000.0 # meters angle_turned: 1000 # radians (1 rad approx 57 deg, approx 60 deg) - voting_active: true - voting_aux_active: false \ No newline at end of file + voting_active: true \ No newline at end of file