From 4537ad25abfd179d6ae0f863dab21cab9bf1e76c Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Fri, 2 Apr 2021 22:25:50 +0200
Subject: [PATCH] working on tdcp
---
include/gnss/processor/processor_gnss_tdcp.h | 2 +-
src/processor/processor_gnss_tdcp.cpp | 25 ++++++---------
src/processor/processor_tracker_gnss.cpp | 32 +++++++++-----------
3 files changed, 26 insertions(+), 33 deletions(-)
diff --git a/include/gnss/processor/processor_gnss_tdcp.h b/include/gnss/processor/processor_gnss_tdcp.h
index 833d258fd..c176ac651 100644
--- a/include/gnss/processor/processor_gnss_tdcp.h
+++ b/include/gnss/processor/processor_gnss_tdcp.h
@@ -41,7 +41,7 @@ struct ParamsProcessorGnssTdcp : public ParamsProcessorGnssFix
tdcp.tdcp.corr_iono = false;
tdcp.relinearize_jacobian = false;
tdcp.max_iterations = 1;
- tdcp.old_nav = false;
+ tdcp.tdcp.use_old_nav = false;
tdcp.tdcp.multi_freq = false;
}
std::string print() const override
diff --git a/src/processor/processor_gnss_tdcp.cpp b/src/processor/processor_gnss_tdcp.cpp
index 1734ecf19..4e27bb919 100644
--- a/src/processor/processor_gnss_tdcp.cpp
+++ b/src/processor/processor_gnss_tdcp.cpp
@@ -165,24 +165,19 @@ void ProcessorGnssTdcp::processKeyFrame(FrameBasePtr _keyframe, const double& _t
WOLF_INFO("Past KF with feature fix found. Performing TDCP!");
// TDCP
- Eigen::Vector4d d;
- Eigen::Matrix4d cov_d;
- double residual;
- std::set<int> discarded_sats, raim_discarded_sats;
- if (GnssUtils::Tdcp(std::static_pointer_cast<CaptureGnss>(cap_gnss_ref)->getSnapshot(),
- std::static_pointer_cast<CaptureGnss>(cap_gnss)->getSnapshot(),
- feat_fix->getMeasurement(),
- d,
- cov_d,
- residual,
- discarded_sats,
- raim_discarded_sats,
- params_tdcp_->tdcp,
- params_tdcp_->compute_pos_opt))
+ std::set<int> discarded_sats;
+ auto tdcp_output = GnssUtils::Tdcp(std::static_pointer_cast<CaptureGnss>(cap_gnss_ref)->getSnapshot(),
+ std::static_pointer_cast<CaptureGnss>(cap_gnss)->getSnapshot(),
+ feat_fix->getMeasurement(),
+ Eigen::Vector4d::Zero(),
+ discarded_sats,
+ params_tdcp_->tdcp,
+ params_tdcp_->compute_pos_opt);
+ if (tdcp_output.success)
{
WOLF_INFO("TDCP successfully performed!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!");
// EMPLACE FEATURE
- auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(cap_gnss, d, cov_d);
+ auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(cap_gnss, tdcp_output.d, tdcp_output.cov_d);
// ADD FACTOR
emplaceFactor(ftr, KF_ref);
diff --git a/src/processor/processor_tracker_gnss.cpp b/src/processor/processor_tracker_gnss.cpp
index 81fc43a2c..02b8fcee3 100644
--- a/src/processor/processor_tracker_gnss.cpp
+++ b/src/processor/processor_tracker_gnss.cpp
@@ -423,33 +423,31 @@ void ProcessorTrackerGnss::establishFactors()
std::cout << sat << " ";
std::cout << std::endl;
+ // reference ECEF position
+ Eigen::Vector3d x_r = sensor_gnss_->computeFrameAntennaPosEcef(KF);
+
// compute TDCP batch
- Eigen::Vector4d d = Eigen::Vector4d::Zero();
- Eigen::Matrix4d cov_d = Eigen::Matrix4d::Identity();
- double residual;
-
- if (GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
- last_cap_gnss->getSnapshot(),
- (fix_last_.success ? fix_last_.pos : Eigen::Vector3d::Zero()),
- common_sats,
- d,
- cov_d,
- residual,
- std::set<int>(),
- params_tracker_gnss_->tdcp_batch_params))
+ auto tdcp_output = GnssUtils::Tdcp(ref_cap_gnss->getSnapshot(),
+ last_cap_gnss->getSnapshot(),
+ x_r,
+ common_sats,
+ Eigen::Vector4d::Zero(),
+ params_tracker_gnss_->tdcp_batch_params);
+ if (tdcp_output.success)
{
- WOLF_DEBUG("TDCP BATCH d = ", d.transpose());
- WOLF_DEBUG("TDCP BATCH cov =\n", cov_d);
+ WOLF_DEBUG("TDCP BATCH d = ", tdcp_output.d.transpose());
+ WOLF_DEBUG("TDCP BATCH cov =\n", tdcp_output.cov_d);
// EMPLACE FEATURE
- auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(d.head<3>()), Eigen::Matrix3d(cov_d.topLeftCorner<3,3>()));
+ auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(last_cap_gnss, Eigen::Vector3d(tdcp_output.d.head<3>()), Eigen::Matrix3d(tdcp_output.cov_d.topLeftCorner<3,3>()));
+ //auto ftr = FeatureBase::emplace<FeatureGnssTdcp>(cap_gnss, tdcp_output.d, tdcp_output.cov_d);
// EMPLACE FACTOR
FactorBase::emplace<FactorGnssTdcp3d>(ftr, ftr, KF, sensor_gnss_, shared_from_this());
}
else
{
- WOLF_DEBUG("TDCP BATCH failed");
+ WOLF_WARN("TDCP BATCH failed with msg: ", tdcp_output.msg);
}
}
}
--
GitLab