diff --git a/src/processor/processor_tracker_feature_landmark_external.cpp b/src/processor/processor_tracker_feature_landmark_external.cpp index 69d09ec77a5c9a7f07879fe8580702aadc18b427..4f4e90e5c78fddec16cddd5c6f77585df3689a1e 100644 --- a/src/processor/processor_tracker_feature_landmark_external.cpp +++ b/src/processor/processor_tracker_feature_landmark_external.cpp @@ -43,6 +43,7 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess() { assert(unmatched_detections_incoming_.empty()); + auto dim = getProblem()->getDim(); auto cap_landmarks = std::dynamic_pointer_cast<CaptureLandmarksExternal>(incoming_ptr_); if (not cap_landmarks) throw std::runtime_error("ProcessorTrackerFeatureLandmarkExternal::preProcess incoming_ptr_ should be of type 'CaptureLandmarksExternal'"); @@ -57,10 +58,29 @@ void ProcessorTrackerFeatureLandmarkExternal::preProcess() or ids.count(detection.id)) 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, "FeatureLandmarkExternal", - detection.measure, - detection.covariance); + meas, + cov); ftr->setLandmarkId(detection.id); if (detection.id != -1 and detection.id != 0)