diff --git a/include/vision/factor/factor_autodiff_trifocal.h b/include/vision/factor/factor_autodiff_trifocal.h
index 71002f224df174a5c8b47f6945d37b09b145ed14..d95aa044d7f25fbdb72c60b98b98090d53321984 100644
--- a/include/vision/factor/factor_autodiff_trifocal.h
+++ b/include/vision/factor/factor_autodiff_trifocal.h
@@ -165,7 +165,6 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_1_
camera_ptr_(std::static_pointer_cast<SensorCamera>(_processor_ptr->getSensor())),
sqrt_information_upper(Matrix3s::Zero())
{
- setFeature(_feature_own_ptr); //< this sets the own feature, the one owning this constraint, which can be seen as feature 3 (newest)
Matrix3s K_inv = camera_ptr_->getIntrinsicMatrix().inverse();
pixel_canonical_1_ = K_inv * Vector3s(_feature_1_ptr->getMeasurement(0), _feature_1_ptr->getMeasurement(1), 1.0);
pixel_canonical_2_ = K_inv * Vector3s(_feature_2_ptr->getMeasurement(0), _feature_2_ptr->getMeasurement(1), 1.0);
@@ -201,9 +200,9 @@ FactorAutodiffTrifocal::FactorAutodiffTrifocal(const FeatureBasePtr& _feature_1_
Matrix<Scalar,3,2> J_e_u3 = J_e_m3 * J_m_u;
// Error covariances induced by each of the measurement covariance // canonical units
- Matrix3s Q1 = J_e_u1 * getFeaturePrev() ->getMeasurementCovariance() * J_e_u1.transpose();
- Matrix3s Q2 = J_e_u2 * getFeatureOther()->getMeasurementCovariance() * J_e_u2.transpose();
- Matrix3s Q3 = J_e_u3 * getFeature() ->getMeasurementCovariance() * J_e_u3.transpose();
+ Matrix3s Q1 = J_e_u1 * _feature_1_ptr ->getMeasurementCovariance() * J_e_u1.transpose(); // FIXME: changed getFeaturePrev() by _feature_1_ptr
+ Matrix3s Q2 = J_e_u2 * _feature_2_ptr ->getMeasurementCovariance() * J_e_u2.transpose(); // FIXME: changed getFeatureOther() by _feature_2_ptr
+ Matrix3s Q3 = J_e_u3 * _feature_own_ptr ->getMeasurementCovariance() * J_e_u3.transpose(); // FIXME: changed getFeature() by _feature_own_ptr
// Total error covariance // canonical units
Matrix3s Q = Q1 + Q2 + Q3;
diff --git a/include/vision/processor/processor_tracker_feature_image.h b/include/vision/processor/processor_tracker_feature_image.h
index 908f397f3b8ab628837f0ecc036c3303b1839bed..a91f46ee1e5c539a4684af309a3cd15cdfbad49d 100644
--- a/include/vision/processor/processor_tracker_feature_image.h
+++ b/include/vision/processor/processor_tracker_feature_image.h
@@ -91,7 +91,17 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
image_last_ = image_incoming_;
}
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
+ * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * \return the number of features tracked
+ */
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
FeatureMatchMap& _feature_correspondences);
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
@@ -113,17 +123,33 @@ class ProcessorTrackerFeatureImage : public ProcessorTrackerFeature
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in preprocess(), you should either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`),
+ * or create them (`make_shared()`) and link all the returned features in _features_out (`FeatureBase::link(_capture)`).
+ *
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out);
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
+ /** \brief Emplaces a new factor
+ * \param _feature_ptr pointer to the parent Feature
+ * \param _feature_other_ptr pointer to the other feature constrained.
+ *
+ * Implement this method in derived classes.
+ *
+ * This function emplaces a factor of the appropriate type for the derived processor.
+ */
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr);
private:
diff --git a/include/vision/processor/processor_tracker_feature_trifocal.h b/include/vision/processor/processor_tracker_feature_trifocal.h
index 51e109cba079af55d3bada4d600c7fedb703ab5e..1742e31fc9f035049a32d7d7b41631a7570c9355 100644
--- a/include/vision/processor/processor_tracker_feature_trifocal.h
+++ b/include/vision/processor/processor_tracker_feature_trifocal.h
@@ -73,12 +73,18 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
virtual ~ProcessorTrackerFeatureTrifocal();
virtual void configure(SensorBasePtr _sensor) override;
- /** \brief Track provided features from \b last to \b incoming
- * \param _features_last_in input list of features in \b last to track
- * \param _features_incoming_out returned list of features found in \b incoming
- * \param _feature_matches returned map of correspondences: _feature_matches[feature_out_ptr] = feature_in_ptr
+ /** \brief Track provided features in \b _capture
+ * \param _features_in input list of features in \b last to track
+ * \param _capture the capture in which the _features_in should be searched
+ * \param _features_out returned list of features found in \b _capture
+ * \param _feature_correspondences returned map of correspondences: _feature_correspondences[feature_out_ptr] = feature_in_ptr
+ *
+ * \return the number of features tracked
*/
- virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_matches) override;
+ virtual unsigned int trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_correspondences) override;
/** \brief Correct the drift in incoming feature by re-comparing against the corresponding feature in origin.
* \param _origin_feature input feature in origin capture tracked
@@ -98,7 +104,8 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
@@ -106,17 +113,17 @@ class ProcessorTrackerFeatureTrifocal : public ProcessorTrackerFeature
* The function is called in ProcessorTrackerFeature::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out) override;
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
- /** \brief Create a new factor and link it to the wolf tree
+ /** \brief Emplaces a new factor
* \param _feature_ptr pointer to the parent Feature
* \param _feature_other_ptr pointer to the other feature constrained.
*
- * Implement this method in derived classes.
- *
- * This function creates a factor of the appropriate type for the derived processor.
+ * This function emplaces a factor of the appropriate type for the derived processor.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr) override;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/include/vision/processor/processor_tracker_landmark_image.h b/include/vision/processor/processor_tracker_landmark_image.h
index 2298c9ea91da400210088850cd49829bfb478fa9..91c43ba6423e538ac6845597d6423f2b7053f023 100644
--- a/include/vision/processor/processor_tracker_landmark_image.h
+++ b/include/vision/processor/processor_tracker_landmark_image.h
@@ -109,14 +109,18 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
*/
void postProcess();
- //Pure virtual
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
+ /** \brief Find provided landmarks as features in the provided capture
+ * \param _landmarks_in input list of landmarks to be found
+ * \param _capture the capture in which the _landmarks_in should be searched
+ * \param _features_out returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
+ *
+ * \return the number of landmarks found
*/
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
+ virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) override;
/** \brief Vote for KeyFrame generation
*
@@ -129,30 +133,36 @@ class ProcessorTrackerLandmarkImage : public ProcessorTrackerLandmark
/** \brief Detect new Features
* \param _max_features maximum number of features detected (-1: unlimited. 0: none)
- * \param _features_last_out The list of detected Features.
+ * \param _capture The capture in which the new features should be detected.
+ * \param _features_out The list of detected Features in _capture.
* \return The number of detected Features.
*
* This function detects Features that do not correspond to known Features/Landmarks in the system.
*
+ * IMPORTANT: The features in _features_out should be emplaced. Don't use `make_shared`, use `FeatureBase::emplace` instead.
+ * Then, they will be already linked to the _capture.
+ * If you detect all the features at once in `preprocess()`, you should either emplace them (`FeatureBase::emplace()`) and remove the not returned features in _features_out (`FeatureBase::remove()`),
+ * or create them (`make_shared()`) and link all the returned features in _features_out (`FeatureBase::link(_capture)`).
+ *
* The function is called in ProcessorTrackerLandmark::processNew() to set the member new_features_last_,
* the list of newly detected features of the capture last_ptr_.
*/
- virtual unsigned int detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out);
+ virtual unsigned int detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out) override;
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
+ /** \brief Emplaces one landmark
*/
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
+ virtual LandmarkBasePtr emplaceLandmark(FeatureBasePtr _feature_ptr) override;
public:
static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
- /** \brief Create a new factor
+ /** \brief Emplaces a new factor
* \param _feature_ptr pointer to the Feature to constrain
* \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
*/
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
+ virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr) override;
//Other functions
private:
diff --git a/src/processor/processor_tracker_feature_image.cpp b/src/processor/processor_tracker_feature_image.cpp
index ab7c59b19932ae1503025d3ee2041e7f504cf81b..57f5c2eba7778411c487c6c2bc5f2f7f117bfe70 100644
--- a/src/processor/processor_tracker_feature_image.cpp
+++ b/src/processor/processor_tracker_feature_image.cpp
@@ -76,14 +76,16 @@ void ProcessorTrackerFeatureImage::postProcess()
{
}
-unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out,
- FeatureMatchMap& _feature_matches)
+unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_matches)
{
KeyPointVector candidate_keypoints;
cv::Mat candidate_descriptors;
DMatchVector cv_matches;
- for (auto feature_base_ptr : _features_last_in)
+ for (auto feature_base_ptr : _features_in)
{
FeaturePointImagePtr feature_ptr = std::static_pointer_cast<FeaturePointImage>(feature_base_ptr);
@@ -102,13 +104,14 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBasePtrLis
Scalar normalized_score = match(target_descriptor,candidate_descriptors,cv_matches);
if ( normalized_score > mat_ptr_->getParams()->min_norm_score )
{
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_matches[0].trainIdx],
- cv_matches[0].trainIdx,
- (candidate_descriptors.row(cv_matches[0].trainIdx)),
- Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
- incoming_point_ptr->setIsKnown(feature_ptr->isKnown());
- _features_incoming_out.push_back(incoming_point_ptr);
+ auto incoming_point_ptr = FeatureBase::emplace<FeaturePointImage>(_capture,
+ candidate_keypoints[cv_matches[0].trainIdx],
+ cv_matches[0].trainIdx,
+ (candidate_descriptors.row(cv_matches[0].trainIdx)),
+ Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
+
+ std::static_pointer_cast<FeaturePointImage>(incoming_point_ptr)->setIsKnown(feature_ptr->isKnown());
+ _features_out.push_back(incoming_point_ptr);
_feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
}
@@ -126,8 +129,8 @@ unsigned int ProcessorTrackerFeatureImage::trackFeatures(const FeatureBasePtrLis
tracker_roi_.pop_back();
}
}
-// std::cout << "TrackFeatures - Number of Features tracked: " << _features_incoming_out.size() << std::endl;
- return _features_incoming_out.size();
+// std::cout << "TrackFeatures - Number of Features tracked: " << _features_out.size() << std::endl;
+ return _features_out.size();
}
bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
@@ -185,7 +188,9 @@ bool ProcessorTrackerFeatureImage::correctFeatureDrift(const FeatureBasePtr _ori
}
}
-unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out)
+unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
{
cv::Rect roi;
KeyPointVector new_keypoints;
@@ -212,13 +217,15 @@ unsigned int ProcessorTrackerFeatureImage::detectNewFeatures(const int& _max_new
if(new_keypoints[0].response > params_tracker_feature_image_activesearch_ptr_->min_response_new_feature)
{
std::cout << "response: " << new_keypoints[0].response << std::endl;
- FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
- new_keypoints[0],
- 0,
- new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
- point_ptr->setIsKnown(false);
- _features_last_out.push_back(point_ptr);
+ auto point_ptr = FeatureBase::emplace<FeaturePointImage>(_capture,
+ new_keypoints[0],
+ 0,
+ new_descriptors.row(index),
+ Eigen::Matrix2s::Identity()*params_tracker_feature_image_->pixel_noise_var);
+
+ std::static_pointer_cast<FeaturePointImage>(point_ptr)->setIsKnown(false);
+
+ _features_out.push_back(point_ptr);
active_search_ptr_->hitCell(new_keypoints[0]);
@@ -247,14 +254,10 @@ Scalar ProcessorTrackerFeatureImage::match(cv::Mat _target_descriptor, cv::Mat _
return normalized_score;
}
-FactorBasePtr ProcessorTrackerFeatureImage::createFactor(FeatureBasePtr _feature_ptr,
+FactorBasePtr ProcessorTrackerFeatureImage::emplaceFactor(FeatureBasePtr _feature_ptr,
FeatureBasePtr _feature_other_ptr)
{
- FactorFeatureDummyPtr const_dummy_ptr = std::make_shared<FactorFeatureDummy>(_feature_ptr, _feature_other_ptr,
- shared_from_this());
- // _feature_ptr->addFactor(const_epipolar_ptr);
- // _feature_other_ptr->addConstrainedBy(const_epipolar_ptr);
- return const_dummy_ptr;
+ return FactorBase::emplace<FactorFeatureDummy>(_feature_ptr, _feature_ptr, _feature_other_ptr, shared_from_this());
}
unsigned int ProcessorTrackerFeatureImage::detect(cv::Mat _image, cv::Rect& _roi, std::vector<cv::KeyPoint>& _new_keypoints,
diff --git a/src/processor/processor_tracker_feature_trifocal.cpp b/src/processor/processor_tracker_feature_trifocal.cpp
index be96cc33f3f466516300ca44686f5cb49d552999..1669a10116bd9895c83189c8fecde258ace8c97c 100644
--- a/src/processor/processor_tracker_feature_trifocal.cpp
+++ b/src/processor/processor_tracker_feature_trifocal.cpp
@@ -81,7 +81,9 @@ bool ProcessorTrackerFeatureTrifocal::isInlier(const cv::KeyPoint& _kp_last, con
}
-unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features, FeatureBasePtrList& _features_last_out)
+unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
{
// // DEBUG =====================================
// clock_t debug_tStart;
@@ -116,14 +118,14 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_
// validate match with extra tests
if (isInlier( kp_incoming, kp_last))
{
- // Create WOLF feature
- FeaturePointImagePtr ftr_point_last = std::make_shared<FeaturePointImage>(
- kp_last,
- index_last,
- capture_image_last_->descriptors_.row(index_last),
- pixel_cov_);
+ // Emplace WOLF feature
+ auto ftr_point_last = FeatureBase::emplace<FeaturePointImage>(_capture,
+ kp_last,
+ index_last,
+ capture_image_last_->descriptors_.row(index_last),
+ pixel_cov_);
- _features_last_out.push_back(ftr_point_last);
+ _features_out.push_back(ftr_point_last);
// hit cell to acknowledge there's a tracked point in that cell
capture_image_last_->grid_features_->hitTrackingCell(kp_last);
@@ -147,10 +149,13 @@ unsigned int ProcessorTrackerFeatureTrifocal::detectNewFeatures(const int& _max_
// WOLF_TRACE("--> TIME: detect new features: TOTAL ",debug_comp_time_);
// WOLF_TRACE("======== END DETECT NEW FEATURES =========");
- return _features_last_out.size();
+ return _features_out.size();
}
-unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBasePtrList& _features_last_in, FeatureBasePtrList& _features_incoming_out, FeatureMatchMap& _feature_matches)
+unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBasePtrList& _features_in,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ FeatureMatchMap& _feature_matches)
{
// // DEBUG =====================================
// clock_t debug_tStart;
@@ -159,7 +164,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBasePtr
// WOLF_TRACE("======== TrackFeatures =========");
// // ===========================================
- for (auto feature_base_last_ : _features_last_in)
+ for (auto feature_base_last_ : _features_in)
{
FeaturePointImagePtr feature_last_ = std::static_pointer_cast<FeaturePointImage>(feature_base_last_);
@@ -175,13 +180,13 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBasePtr
if (isInlier(kp_last, kp_incoming))
{
- FeaturePointImagePtr ftr_point_incoming = std::make_shared<FeaturePointImage>(
- kp_incoming,
- index_incoming,
- capture_image_incoming_->descriptors_.row(index_incoming),
- pixel_cov_);
+ auto ftr_point_incoming = FeatureBase::emplace<FeaturePointImage>(_capture,
+ kp_incoming,
+ index_incoming,
+ capture_image_incoming_->descriptors_.row(index_incoming),
+ pixel_cov_);
- _features_incoming_out.push_back(ftr_point_incoming);
+ _features_out.push_back(ftr_point_incoming);
_feature_matches[ftr_point_incoming] = std::make_shared<FeatureMatch>(FeatureMatch({feature_last_, capture_image_incoming_->matches_normalized_scores_.at(index_incoming)}));
@@ -198,7 +203,7 @@ unsigned int ProcessorTrackerFeatureTrifocal::trackFeatures(const FeatureBasePtr
// WOLF_TRACE("--> TIME: track: ",debug_comp_time_);
// WOLF_TRACE("======== END TRACK FEATURES =========");
- return _features_incoming_out.size();
+ return _features_out.size();
}
bool ProcessorTrackerFeatureTrifocal::correctFeatureDrift(const FeatureBasePtr _origin_feature, const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature)
@@ -358,19 +363,19 @@ void ProcessorTrackerFeatureTrifocal::postProcess()
cv::waitKey(1);
}
-FactorBasePtr ProcessorTrackerFeatureTrifocal::createFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
+FactorBasePtr ProcessorTrackerFeatureTrifocal::emplaceFactor(FeatureBasePtr _feature_ptr, FeatureBasePtr _feature_other_ptr)
{
// NOTE: This function cannot be implemented because the API lacks an input to feature_prev_origin.
// Therefore, we implement establishFactors() instead and do all the job there.
// This function remains empty, and with a warning or even an error issued in case someone calls it.
- std::cout << "033[1;33m [WARN]:033[0m ProcessorTrackerFeatureTrifocal::createFactor is empty." << std::endl;
+ std::cout << "033[1;33m [WARN]:033[0m ProcessorTrackerFeatureTrifocal::emplaceFactor is empty." << std::endl;
FactorBasePtr return_var{}; //TODO: fill this variable
return return_var;
}
void ProcessorTrackerFeatureTrifocal::establishFactors()
{
-// WOLF_TRACE("===== ESTABLISH CONSTRAINT =====");
+// WOLF_TRACE("===== ESTABLISH FACTORS =====");
if (initialized_)
{
@@ -426,13 +431,13 @@ void ProcessorTrackerFeatureTrifocal::establishFactors()
assert(ftr_mid != ftr_first && "First and middle features are the same!");
assert(ftr_mid != ftr_last && "Last and middle features are the same!");
- // make factor
+ // emplace factor
auto ctr = FactorBase::emplace<FactorAutodiffTrifocal>(ftr_last, ftr_first, ftr_mid, ftr_last, shared_from_this(), false, FAC_ACTIVE);
}
}
}
-// WOLF_TRACE("===== END ESTABLISH CONSTRAINT =====");
+// WOLF_TRACE("===== END ESTABLISH FACTORS =====");
}
diff --git a/src/processor/processor_tracker_landmark_image.cpp b/src/processor/processor_tracker_landmark_image.cpp
index b9ae150849c2af1d3165ddfeb36a7978d169c616..bd7de009d94a89309690c6d224a741fa7c6e0c1f 100644
--- a/src/processor/processor_tracker_landmark_image.cpp
+++ b/src/processor/processor_tracker_landmark_image.cpp
@@ -87,8 +87,9 @@ void ProcessorTrackerLandmarkImage::postProcess()
}
unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences)
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences)
{
KeyPointVector candidate_keypoints;
cv::Mat candidate_descriptors;
@@ -125,18 +126,19 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrL
if (normalized_score > mat_ptr_->getParams()->min_norm_score )
{
- FeaturePointImagePtr incoming_point_ptr = std::make_shared<FeaturePointImage>(
- candidate_keypoints[cv_matches[0].trainIdx],
- cv_matches[0].trainIdx,
- candidate_descriptors.row(cv_matches[0].trainIdx),
- Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
+ FeaturePointImagePtr incoming_point_ptr = std::static_pointer_cast<FeaturePointImage>(
+ FeatureBase::emplace<FeaturePointImage>(_capture,
+ candidate_keypoints[cv_matches[0].trainIdx],
+ cv_matches[0].trainIdx,
+ candidate_descriptors.row(cv_matches[0].trainIdx),
+ Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var) );
incoming_point_ptr->setTrackId(landmark_in_ptr->id());
incoming_point_ptr->setLandmarkId(landmark_in_ptr->id());
incoming_point_ptr->setScore(normalized_score);
incoming_point_ptr->setExpectation(pixel);
- _features_incoming_out.push_back(incoming_point_ptr);
+ _features_out.push_back(incoming_point_ptr);
_feature_landmark_correspondences[incoming_point_ptr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, normalized_score);
@@ -157,8 +159,8 @@ unsigned int ProcessorTrackerLandmarkImage::findLandmarks(const LandmarkBasePtrL
// std::cout << "NOT in the image" << std::endl;
}
// std::cout << "\tNumber of Features tracked: " << _features_incoming_out.size() << std::endl;
- landmarks_tracked_ = _features_incoming_out.size();
- return _features_incoming_out.size();
+ landmarks_tracked_ = _features_out.size();
+ return _features_out.size();
}
bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
@@ -167,7 +169,9 @@ bool ProcessorTrackerLandmarkImage::voteForKeyFrame()
// return landmarks_tracked_ < params_tracker_landmark_image_->min_features_for_keyframe;
}
-unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_features, FeatureBasePtrList& _features_last_out)
+unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_new_features,
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out)
{
cv::Rect roi;
KeyPointVector new_keypoints;
@@ -175,7 +179,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_fe
cv::KeyPointsFilter keypoint_filter;
unsigned int n_new_features = 0;
- for (unsigned int n_iterations = 0; _max_features == -1 || n_iterations < _max_features; n_iterations++)
+ for (unsigned int n_iterations = 0; _max_new_features == -1 || n_iterations < _max_new_features; n_iterations++)
{
if (active_search_ptr_->pickEmptyRoi(roi))
{
@@ -194,15 +198,16 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_fe
if(new_keypoints[0].response > params_tracker_landmark_image_activesearch_ptr_->min_response_new_feature)
{
list_response_.push_back(new_keypoints[0].response);
- FeaturePointImagePtr point_ptr = std::make_shared<FeaturePointImage>(
- new_keypoints[0],
- 0,
- new_descriptors.row(index),
- Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var);
+ FeaturePointImagePtr point_ptr = std::static_pointer_cast<FeaturePointImage>(
+ FeatureBase::emplace<FeaturePointImage>(_capture,
+ new_keypoints[0],
+ 0,
+ new_descriptors.row(index),
+ Eigen::Matrix2s::Identity()*params_tracker_landmark_image_->pixel_noise_var) );
point_ptr->setIsKnown(false);
point_ptr->setTrackId(point_ptr->id());
point_ptr->setExpectation(Eigen::Vector2s(new_keypoints[0].pt.x,new_keypoints[0].pt.y));
- _features_last_out.push_back(point_ptr);
+ _features_out.push_back(point_ptr);
active_search_ptr_->hitCell(new_keypoints[0]);
@@ -221,7 +226,7 @@ unsigned int ProcessorTrackerLandmarkImage::detectNewFeatures(const int& _max_fe
return n_new_features;
}
-LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _feature_ptr)
+LandmarkBasePtr ProcessorTrackerLandmarkImage::emplaceLandmark(FeatureBasePtr _feature_ptr)
{
FeaturePointImagePtr feat_point_image_ptr = std::static_pointer_cast<FeaturePointImage>( _feature_ptr);
@@ -245,15 +250,19 @@ LandmarkBasePtr ProcessorTrackerLandmarkImage::createLandmark(FeatureBasePtr _fe
vec_homogeneous = {point3D(0),point3D(1),point3D(2),1/distance};
- LandmarkAHPPtr lmk_ahp_ptr = std::make_shared<LandmarkAHP>(vec_homogeneous, anchor_frame, getSensor(), feat_point_image_ptr->getDescriptor());
+ auto lmk_ahp_ptr = LandmarkBase::emplace<LandmarkAHP>(getProblem()->getMap(),
+ vec_homogeneous,
+ anchor_frame,
+ getSensor(),
+ feat_point_image_ptr->getDescriptor());
_feature_ptr->setLandmarkId(lmk_ahp_ptr->id());
return lmk_ahp_ptr;
}
-FactorBasePtr ProcessorTrackerLandmarkImage::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
+FactorBasePtr ProcessorTrackerLandmarkImage::emplaceFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
{
- if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFrame())
+ if ((std::static_pointer_cast<LandmarkAHP>(_landmark_ptr))->getAnchorFrame() == last_ptr_->getFrame()) //FIXME: shouldn't it be _feature_ptr->getFrame() ?
{
return FactorBasePtr();
}
@@ -264,9 +273,7 @@ FactorBasePtr ProcessorTrackerLandmarkImage::createFactor(FeatureBasePtr _featur
LandmarkAHPPtr landmark_ahp = std::static_pointer_cast<LandmarkAHP>(_landmark_ptr);
- FactorAHPPtr factor_ptr = FactorAHP::create(_feature_ptr, landmark_ahp, shared_from_this(), true);
-
- return factor_ptr;
+ return FactorBase::emplace<FactorAHP>(_feature_ptr, _feature_ptr, landmark_ahp, shared_from_this(), true);
}
}