diff --git a/include/objectslam/processor/processor_tracker_landmark_object.h b/include/objectslam/processor/processor_tracker_landmark_object.h
index c054322b5d14dee0f7be4b2e8b08ce721661a66a..1aaf437194c4e1cd88c563a55d1de9412e97600f 100644
--- a/include/objectslam/processor/processor_tracker_landmark_object.h
+++ b/include/objectslam/processor/processor_tracker_landmark_object.h
@@ -6,7 +6,7 @@
#include <core/processor/processor_tracker_landmark.h>
#include <core/factor/factor_distance_3d.h>
#include "objectslam/landmark/landmark_object.h"
-
+#include "core/processor/track_matrix.h"
namespace wolf
@@ -96,9 +96,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
* \return the number of landmarks found
*/
unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in,
- const CaptureBasePtr& _capture,
- FeatureBasePtrList& _features_out,
- LandmarkMatchMap& _feature_landmark_correspondences) override;
+ const CaptureBasePtr& _capture,
+ FeatureBasePtrList& _features_out,
+ LandmarkMatchMap& _feature_landmark_correspondences) override;
/** \brief First method for matching lmk and feat
* \param _landmarks_in input list of landmarks to be found
* \param _capture the capture in which the _landmarks_in should be searched
@@ -146,13 +146,13 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList& _features_out_last,
FeatureBasePtrList& _features_out_incoming);
- static bool matchingRANSAC(const std::vector<Eigen::Isometry3d>& cl_M_o_vec,
- const std::vector<Eigen::Isometry3d>& ci_M_o_vec,
- const std::vector<std::pair<int,int> >& matches,
- std::vector<int>& inliers_idx,
- std::vector<int>& outliers_idx,
- Eigen::Isometry3d& best_model,
- double ratio_outliers_inliers);
+ static bool matchingRANSAC(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
+ const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
+ const std::vector<std::pair<int,int> >& _matches,
+ std::vector<int>& _inliers_idx,
+ std::vector<int>& _outliers_idx,
+ Eigen::Isometry3d& _best_model,
+ double _ratio_outliers_inliers);
/** \brief Count the number of different matches
* \param matches A vector of pair of matches between last and incoming
@@ -161,7 +161,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
*
* \return an int
*/
- static int nbOfDifferentMatches(const std::vector<std::pair<int,int> >& matches);
+ static int nbOfDifferentMatches(const std::vector<std::pair<int,int> >& _matches);
/** \brief Check if three transformations correspond to an inlier
* \param ci_M_oi camera pose isometry last to incoming
@@ -172,10 +172,17 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
*
* \return A bool
*/
- static bool isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci);
+ static bool isInliers(Eigen::Isometry3d _cl_M_ol, Eigen::Isometry3d _ci_M_oi, Eigen::Isometry3d _cl_M_ci);
+
+ static void processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered,
+ TrackMatrix& _trackMatrix,
+ const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming);
static void filterMatches(std::vector<std::pair<int,int> >& matches, const std::vector<int>& outliers_idx);
+ const TrackMatrix& getTrackMatrix() const {return track_matrix_;}
+
/** \brief Vote for KeyFrame generation
*
* If a KeyFrame criterion is validated, this function returns true,
@@ -242,6 +249,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
bool first_lmk_matching_;
double ratio_inliers_outliers_;
+ TrackMatrix track_matrix_;
+
+
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
};
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 1c30a796b201d3abfbe120fd5409c718274549a2..76477e711f6470409b692e60bbdfa226dd5a101e 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -62,8 +62,8 @@ void ProcessorTrackerLandmarkObject::postProcess()
std::list<LandmarkBasePtr> lmk_to_remove;
// Clear the landmark map
- if (keyframe_voted_){
- // Only if a keyframe is voted so that a suppressed landmark is not assigned to a factor
+ if (keyframe_voted_){
+ // Only if a keyframe is voted so that a suppressed landmark is not assigned to a factor
for (auto lmk: getProblem()->getMap()->getLandmarkList()){
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
@@ -89,7 +89,7 @@ void ProcessorTrackerLandmarkObject::postProcess()
FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feature_ptr,
LandmarkBasePtr _landmark_ptr)
{
- // A keyframe is voted
+ // A keyframe is voted
keyframe_voted_ = true;
return FactorBase::emplace<FactorRelativePose3dWithExtrinsics>(_feature_ptr,
@@ -139,7 +139,7 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
// max_new_features reached
if (_max_new_features != -1 && _features_out.size() == _max_new_features)
break;
-
+
bool feature_already_found = false;
auto feat_obj = std::static_pointer_cast<FeatureObject>(feat);
@@ -165,7 +165,7 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
}
bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
-{
+{
// A few variables to examine the state of the system
// Feature detection wise
@@ -173,7 +173,7 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
bool too_few_detections_incoming = detections_incoming_.size() < min_features_for_keyframe_;
// Feature-Landmark matching wise
- // TODO:
+ // TODO:
// Ideally we would need to use the size of the output of findLandmarks: _features_out
// -> number of matched features after processKnown
// Stored in matches_landmark_from_incoming_ (ProcessorTrackerLandmark)
@@ -181,7 +181,7 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
// Time wise
double dt_incoming_origin = getIncoming()->getTimeStamp().get() - getOrigin()->getTimeStamp().get();
- bool enough_time_vote = dt_incoming_origin > min_time_vote_;
+ bool enough_time_vote = dt_incoming_origin > min_time_vote_;
bool too_long_since_origin_KF = dt_incoming_origin > max_time_vote_;
WOLF_INFO("dt_incoming_origin: ", getOrigin()->id(), " -> ", getIncoming()->id(), " dt : ", dt_incoming_origin)
@@ -242,7 +242,7 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
}
for (auto lmk : _landmarks_in)
- {
+ {
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
//Try matching only if lmk and feat have the same type
@@ -250,18 +250,18 @@ unsigned int ProcessorTrackerLandmarkObject::firstLmkMatching(const LandmarkBase
{
//world to feat
Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
-
+
//Create a pair that stock both position and rotation error
auto error = errorsComputations(w_M_f, lmk_obj);
- //Test errors are below thresholds
+ //Test errors are below thresholds
if (error.first < e_pos_thr_ && error.second < e_rot_thr_){
_features_out.push_back(feat);
double score(1.0); // not used
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(lmk, score);
_feature_landmark_correspondences.emplace (feat, matched_landmark);
- feat->link(_capture); // since all features are created in preProcess() are unlinked
- break;
+ feat->link(_capture); // since all features are created in preProcess() are unlinked
+ break;
}
}
}
@@ -302,7 +302,7 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector("PO"));
// rob to sensor
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
-
+
//If there is not lmk no need to try matching
if (_landmarks_in.size() == 0)
return _features_out.size();
@@ -312,11 +312,11 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
// camera to feat
Eigen::Isometry3d c_M_f = posevec_to_isometry(feat->getMeasurement());
-
+
if (feat->getCapture() != nullptr){
break;
}
-
+
//Counter for _landmark_in
int i = 0;
//Variable to store the best lmk match
@@ -329,7 +329,7 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
double e_rot_min = 100;
for (auto lmk : _landmarks_in)
- {
+ {
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
{
@@ -338,33 +338,33 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
//Create a pair that stock both position and rotation error
auto error = errorsComputations(w_M_f, lmk_obj);
- //Test if the error is below the thresholds
+ //Test if the error is below the thresholds
if (error.first < e_pos_thr_ && error.second < e_rot_thr_)
- {
+ {
//Test if errors are below the previous min errors
if (error.first < e_pos_min && error.second < e_rot_min)
{
e_pos_min = error.first;
e_rot_min = error.second;
- index_min = i;
- match = true;
- }
+ index_min = i;
+ match = true;
+ }
}
}
i++;
}
-
+
if (match)
{
- auto itr_lmk = _landmarks_in.begin();
+ auto itr_lmk = _landmarks_in.begin();
std::advance(itr_lmk, index_min);
_features_out.push_back(feat);
double score(1.0); // not used
LandmarkMatchPtr matched_landmark = std::make_shared<LandmarkMatch>(*itr_lmk, score);
_feature_landmark_correspondences.emplace (feat, matched_landmark);
- feat->link(_capture); // since all features are created in preProcess() are unlinked
+ feat->link(_capture); // since all features are created in preProcess() are unlinked
match = false;
}
}
@@ -373,7 +373,7 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
return _features_out.size();
}
-
+
unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtrList& _landmarks_in,
const CaptureBasePtr& _capture,
@@ -388,13 +388,13 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
ftr_size = firstLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
}
- //Best lmk in list (lower than threshold) is matched
+ //Best lmk in list (lower than threshold) is matched
else
{
ftr_size = bestLmkMatching(_landmarks_in, _capture, _features_out, _feature_landmark_correspondences);
}
-
+
std::cout << "Features Matched :" << ftr_size << std::endl;
return ftr_size;
}
@@ -417,7 +417,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector("PO"));
std::vector<Eigen::Isometry3d> cl_M_o_vec;
- std::vector<Eigen::Isometry3d> ci_M_o_vec;
+ std::vector<Eigen::Isometry3d> ci_M_o_vec;
std::vector<std::pair<int,int> > matches;
int index_last = 0;
@@ -432,7 +432,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
Eigen::Isometry3d cl_M_o = Eigen::Translation<double,3>(pos_obj_last) * quat_obj_last;
cl_M_o_vec.push_back(cl_M_o);
-
+
Eigen::Isometry3d w_M_f_last = w_M_r_last * r_M_c * cl_M_o;
Vector3d pos_feat_last = w_M_f_last.translation();
@@ -446,7 +446,7 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
if (object_type_last == object_type_incoming)
- {
+ {
//camera to feat
Vector3d pos_obj_incoming = feat_incoming->getMeasurement().head(3);
Quaterniond quat_obj_incoming (feat_incoming->getMeasurement().tail(4).data());
@@ -484,16 +484,60 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
bool RANSACWorks = ProcessorTrackerLandmarkObject::matchingRANSAC(cl_M_o_vec, ci_M_o_vec, matches, inliers_idx, outliers_idx, best_model, ratio_inliers_outliers_);
if (RANSACWorks)
- {
+ {
std::cout << "RANSAC has worked" << std::endl;
- ProcessorTrackerLandmarkObject::filterMatches(matches, outliers_idx);
- // int nb_outliers = outliers_idx.size();
- // Eigen::Isometry3d cf_M_ci = best_model;
+
+ //Keep only inliers
+ ProcessorTrackerLandmarkObject::filterMatches(matches, inliers_idx);
+
}
-
+
return 1;
}
+void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair<int,int> >& _matches_filtered,
+ TrackMatrix& _trackMatrix,
+ const FeatureBasePtrList& _features_out_last,
+ const FeatureBasePtrList& _features_out_incoming)
+{
+ for (auto match : _matches_filtered)
+ {
+ auto feat_last_itr = _features_out_last.begin();
+ auto feat_incoming_itr = _features_out_incoming.begin();
+
+ std::advance(feat_last_itr, match.first);
+ std::advance(feat_incoming_itr, match.second);
+
+ auto feat_last = *feat_last_itr;
+ auto feat_incoming = *feat_incoming_itr;
+
+ auto trackID_feat_last = feat_last->trackId();
+
+ //Test if the track is defined
+ if (trackID_feat_last != 0)
+ {
+ //Add incoming feature to the track
+ _trackMatrix.add(feat_last, feat_incoming);
+
+ auto size_track_feat_last = _trackMatrix.trackSize(trackID_feat_last);
+
+ if (size_track_feat_last >= 3)
+ {
+ //Create Lmk and join it to feature_last(= match.first)
+ }
+ }
+
+ //New feature or track lost
+ else
+ {
+ //Check if the feature can be linked to a previous defined lmk
+
+ //else we create a new track : _trackMatrix.newTrack(match.first) && _trackMatrix.add(match.first, match.second);
+
+ }
+ }
+}
+
std::ostream &operator<<(std::ostream &flux, std::vector<int> vect)
{
for (int element : vect)
@@ -513,50 +557,50 @@ std::ostream &operator<<(std::ostream &flux, std::vector<std::pair<int,int> > ve
}
flux << '\n';
-
+
return flux;
}
-void ProcessorTrackerLandmarkObject::filterMatches(std::vector<std::pair<int,int> >& matches,
- const std::vector<int>& inliers_idx)
+void ProcessorTrackerLandmarkObject::filterMatches(std::vector<std::pair<int,int> >& _matches,
+ const std::vector<int>& _inliers_idx)
{
std::vector<std::pair<int,int> > matches_temp;
- for (auto pair_indexes : matches)
- {
- int index_outlier_occurrence = std::count(inliers_idx.begin(), inliers_idx.end(), pair_indexes.first);
-
+ for (auto pair_indexes : _matches)
+ {
+ int index_outlier_occurrence = std::count(_inliers_idx.begin(), _inliers_idx.end(), pair_indexes.first);
+
if (index_outlier_occurrence != 0)
- {
+ {
matches_temp.push_back(pair_indexes);
}
}
- matches.clear();
- matches = matches_temp;
+ _matches.clear();
+ _matches = matches_temp;
}
-bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Isometry3d>& cl_M_o_vec,
- const std::vector<Eigen::Isometry3d>& ci_M_o_vec,
- const std::vector<std::pair<int,int> >& matches,
- std::vector<int>& inliers_idx,
- std::vector<int>& outliers_idx,
- Eigen::Isometry3d& best_model,
- double ratio_outliers_inliers)
-{
+bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Isometry3d>& _cl_M_o_vec,
+ const std::vector<Eigen::Isometry3d>& _ci_M_o_vec,
+ const std::vector<std::pair<int,int> >& _matches,
+ std::vector<int>& _inliers_idx,
+ std::vector<int>& _outliers_idx,
+ Eigen::Isometry3d& _best_model,
+ double _ratio_outliers_inliers)
+{
//Check if the dataset has a sufficient size
- if (nbOfDifferentMatches(matches) < 3)
+ if (nbOfDifferentMatches(_matches) < 3)
return false;
// Vector that will contains index of inliers/outliers for each iteration
std::vector<int> inliers_idx_buff;
std::vector<int> outliers_idx_buff;
-
+
//Number of inliers for the best model
int best_nb_inliers = 0;
- for (auto match : matches)
+ for (auto match : _matches)
{
//Number of inliers for this model
int nb_inliers = 0;
@@ -566,23 +610,23 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
int index_feat_incoming = match.second;
//Transformation between camera and object in last/incoming frame for this model
- Eigen::Isometry3d cl_M_o = cl_M_o_vec[index_feat_last];
- Eigen::Isometry3d o_M_ci = ci_M_o_vec[index_feat_incoming].inverse();
+ Eigen::Isometry3d cl_M_o = _cl_M_o_vec[index_feat_last];
+ Eigen::Isometry3d o_M_ci = _ci_M_o_vec[index_feat_incoming].inverse();
//Camera's transformation between last and incoming for this model
Eigen::Isometry3d cl_M_ci = cl_M_o * o_M_ci;
- for (auto other_match : matches)
- {
+ for (auto other_match : _matches)
+ {
int index_feat_last_other = other_match.first;
int index_feat_incomming_other = other_match.second;
//Not necessary to test same pair of matches
if (index_feat_last != index_feat_last_other && index_feat_incoming != index_feat_incomming_other)
- {
+ {
//Transformation between camera and object in last/incoming frame for the other match
- Eigen::Isometry3d cl_M_ol = cl_M_o_vec[index_feat_last_other];
- Eigen::Isometry3d ci_M_oi = ci_M_o_vec[index_feat_incomming_other];
+ Eigen::Isometry3d cl_M_ol = _cl_M_o_vec[index_feat_last_other];
+ Eigen::Isometry3d ci_M_oi = _ci_M_o_vec[index_feat_incomming_other];
if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
{
@@ -602,26 +646,26 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
if (nb_inliers > best_nb_inliers)
{
best_nb_inliers = nb_inliers;
- inliers_idx = inliers_idx_buff;
- outliers_idx = outliers_idx_buff;
- best_model = cl_M_ci;
+ _inliers_idx = inliers_idx_buff;
+ _outliers_idx = outliers_idx_buff;
+ _best_model = cl_M_ci;
}
//Buffers clearing
inliers_idx_buff.clear();
outliers_idx_buff.clear();
- }
+ }
- int nb_inliers = inliers_idx.size();
- int nb_outliers = outliers_idx.size();
+ int nb_inliers = _inliers_idx.size();
+ int nb_outliers = _outliers_idx.size();
- if ((double)nb_outliers/nb_inliers > ratio_outliers_inliers)
+ if ((double)nb_outliers/nb_inliers > _ratio_outliers_inliers)
return true;
return false;
}
-int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::pair<int,int> >& matches)
+int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::pair<int,int> >& _matches)
{
int nb_of_diff_matches = 0;
@@ -629,12 +673,12 @@ int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::
std::vector<int> index_last_feat;
std::vector<int> index_incoming_feat;
- for (auto match : matches)
- {
+ for (auto match : _matches)
+ {
//Nb of occurences of indexes
int index_last_feat_occurrence = std::count(index_last_feat.begin(), index_last_feat.end(), match.first);
int index_incoming_feat_occurrence = std::count(index_incoming_feat.begin(), index_incoming_feat.end(), match.second);
-
+
//Check if the pair has unique indexes
if(index_last_feat_occurrence == 0 && index_incoming_feat_occurrence == 0)
{
@@ -650,14 +694,14 @@ int ProcessorTrackerLandmarkObject::nbOfDifferentMatches(const std::vector<std::
}
-bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci)
+bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d _cl_M_ol, Eigen::Isometry3d _ci_M_oi, Eigen::Isometry3d _cl_M_ci)
{
//Thresholds
double e_pos_th = 1;
double e_rot_th = 1;
- Eigen::Isometry3d ol_M_cl = cl_M_ol.inverse();
- Eigen::Isometry3d ol_M_oi = ol_M_cl * cl_M_ci * ci_M_oi;
+ Eigen::Isometry3d ol_M_cl = _cl_M_ol.inverse();
+ Eigen::Isometry3d ol_M_oi = ol_M_cl * _cl_M_ci * _ci_M_oi;
Quaterniond quat_feat;
Eigen::Matrix3d wRf = ol_M_oi.linear();
@@ -697,7 +741,7 @@ void ProcessorTrackerLandmarkObject::advanceDerived()
}
void ProcessorTrackerLandmarkObject::resetDerived()
-{
+{
ProcessorTrackerLandmark::resetDerived();
detections_last_ = std::move(detections_incoming_);
}
diff --git a/test/gtest_processor_tracker_landmark_object.cpp b/test/gtest_processor_tracker_landmark_object.cpp
index 0c781bb034c64db3c2dc8749c6b393a918931b4f..ffd71a617fad4aa6c730303ee6d53ba8009a1a54 100644
--- a/test/gtest_processor_tracker_landmark_object.cpp
+++ b/test/gtest_processor_tracker_landmark_object.cpp
@@ -330,6 +330,7 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Vector7d pose_object_4;
Vector7d pose_object_5;
+ //Give random values
pose_cam_last << 4.1,3.5,0, 0.8,2.7,1.3,1;
pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
pose_object_1 << 3.2,2.1,4.3,5.5, 3.3,2.4,1;
@@ -384,12 +385,14 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
+ //Create 5 matches
auto pair_o1 = std::make_pair(0, 0);
auto pair_o2 = std::make_pair(1, 1);
auto pair_o3 = std::make_pair(2, 2);
auto pair_o4 = std::make_pair(3, 3);
auto pair_o5 = std::make_pair(4, 4);
+ //Append pairs in matches object
matches.push_back(pair_o1);
matches.push_back(pair_o2);
matches.push_back(pair_o3);
@@ -419,8 +422,6 @@ TEST_F(ProcessorTrackerLandmarkObject_fixture, matchingRANSAC)
TEST(ProcessorTrackerLandmarkObject, isInliers)
{
- std::cout << "test11" << "\n\n";
-
//Camera poses
Vector7d pose_cam_last;
Vector7d pose_cam_incoming;
@@ -429,8 +430,8 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
Vector7d pose_object_1;
Vector7d pose_object_2;
-
- pose_cam_last << 4.1,3.5,0, 0.8,2.7,1.3,1;
+ //Give random values
+ pose_cam_last << 4.1,3.5,0,0.8, 2.7,1.3,1;
pose_cam_incoming << 5.2,4.8,0.5,0.4, 2.0,2.1,1.2;
pose_object_1 << 3.2,2.1,4.3,5.5, 3.3,2.4,1;
pose_object_2 << 2.2,1.1,3.3,4.5, 2.3,1.4,0.8;
@@ -464,15 +465,13 @@ TEST(ProcessorTrackerLandmarkObject, isInliers)
ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o3, ci_M_o3, cl_M_ci));
ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o2, cl_M_ci))); //outliers
ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, cl_M_ci))); //outliers
-
- std::cout << "test1" << "\n\n";
}
TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
{
- std::cout << "test" << "\n\n";
std::vector<std::pair<int,int> > matches;
+ //Create 12 matches
auto pair_o1 = std::make_pair(0, 0);
auto pair_o2 = std::make_pair(1, 1);
auto pair_o3 = std::make_pair(2, 2);
@@ -486,6 +485,7 @@ TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
auto pair_o11 = std::make_pair(3, 7);
auto pair_o12 = std::make_pair(6, 7);
+ //Append pairs in matches object
matches.push_back(pair_o1);
matches.push_back(pair_o2);
matches.push_back(pair_o3);
@@ -499,10 +499,7 @@ TEST(ProcessorTrackerLandmarkObject, nbOfDifferentMatches)
matches.push_back(pair_o11);
matches.push_back(pair_o12);
- std::cout << "test" << "\n\n";
- std::cout << ProcessorTrackerLandmarkObject::nbOfDifferentMatches(matches) << "\n\n\n";
-
- ASSERT_TRUE(ProcessorTrackerLandmarkObject::nbOfDifferentMatches(matches) == 6);
+ ASSERT_TRUE(ProcessorTrackerLandmarkObject::nbOfDifferentMatches(matches) == 7);
}