Skip to content
Snippets Groups Projects
Commit 232d361e authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

taking only needed measure and covariance

parent a508947c
No related branches found
No related tags found
1 merge request!466devel->main
...@@ -43,6 +43,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess() ...@@ -43,6 +43,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
{ {
assert(unmatched_detections_incoming_.empty()); assert(unmatched_detections_incoming_.empty());
auto dim = getProblem()->getDim();
auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_); auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_);
if (not cap_landmarks) if (not cap_landmarks)
throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'"); throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'");
...@@ -57,10 +58,29 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess() ...@@ -57,10 +58,29 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess()
or ids.count(detection.id)) or ids.count(detection.id))
continue; continue;
Eigen::VectorXd meas;
Eigen::MatrixXd cov;
if (not params_tfle_->use_orientation)
{
assert(detection.measure.size() >= dim);
assert(detection.covariance.rows() >= dim and detection.covariance.rows() == detection.covariance.cols());
meas = detection.measure.head(dim);
cov = detection.covariance.topLeftCorner(dim,dim);
}
else
{
assert(detection.measure.size() == (dim == 2 ? 3 : 7));
assert(detection.covariance.rows() == (dim == 2 ? 3 : 6) and detection.covariance.rows() == detection.covariance.cols());
meas = detection.measure;
cov = detection.covariance;
}
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks, FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(cap_landmarks,
"FeatureLandmarkExternal", "FeatureLandmarkExternal",
detection.measure, meas,
detection.covariance); cov);
ftr->setLandmarkId(detection.id); ftr->setLandmarkId(detection.id);
if (detection.id != -1 and detection.id != 0) if (detection.id != -1 and detection.id != 0)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment