Add test for matching functions

include/objectslam/processor/processor_tracker_landmark_object.h

@@ -144,9 +144,9 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
                                                  FeatureBasePtrList& _features_out_last,
                                                  FeatureBasePtrList& _features_out_incoming);
 
-        std::vector<int> matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming);
+        static std::pair<std::vector<int>, Eigen::Isometry3d> matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming);
 
-        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);
         
 
         /** \brief Vote for KeyFrame generation
@@ -220,4 +220,5 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
 
 } // namespace wolf
 
+
 #endif /* _PROCESSOR_TRACKER_LANDMARK_OBJECT_H_ */

src/processor/processor_tracker_landmark_object.cpp

+#include <iostream>
+#include <ostream>
+
 #include "core/math/rotations.h"
 #include <core/factor/factor_distance_3d.h>
 #include "core/factor/factor_relative_pose_3d_with_extrinsics.h"
@@ -439,9 +442,10 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
         int index_min_incoming = -1;
         double min_e_pos = 100;
 
+        int index_incoming = 0;
+
         for (auto feat_incoming : detections_incoming_)
         {
-            int index_incoming = 0;
 
             std::string object_type_incoming = std::static_pointer_cast<FeatureObject>(feat_incoming)->getObjectType();
 
@@ -467,20 +471,23 @@ unsigned int ProcessorTrackerLandmarkObject::multiviewTypeMatching(const Capture
                 last_incoming.push_back(tuple_last_incom);
 
             }
+            index_incoming++;
         }
         
         std::cout << index_min_incoming;
+        index_last++;
 
     }
     return 1;
 }
 
-std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming)
+std::pair<std::vector<int>, Eigen::Isometry3d> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming)
 {   
     // Vector that will contains index of outliers for each iteration
     std::vector<std::vector<int> > index_outliers;
     int i = 0;
     int best_score = 0;
+    Eigen::Isometry3d best_cl_M_ci = Eigen::Isometry3d::Identity();;
     int index_best_score = -1;
 
 
@@ -506,13 +513,12 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
                 int index_feat_last = std::get<0>(last_incoming_tuple);
                 int index_feat_last_other = std::get<0>(last_incoming_tuple_other);
                 
-                //Check if there is a match and if 
                 if (index_feat_incoming != -1 && index_feat_last != index_feat_last_other)
                 {   
                     Eigen::Isometry3d cl_M_ol = std::get<2>(last_incoming_tuple_other);
                     Eigen::Isometry3d ci_M_oi = std::get<3>(last_incoming_tuple_other);
 
-                    if(isInliers(cl_M_ol, cl_M_ci, ci_M_oi))
+                    if(isInliers(cl_M_ol, ci_M_oi, cl_M_ci))
                     {
                         score++;
                     }
@@ -527,6 +533,7 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
             {
                 best_score = score;
                 index_best_score = i;
+                best_cl_M_ci = cl_M_ci;
             }
 
         }
@@ -537,16 +544,33 @@ std::vector<int> ProcessorTrackerLandmarkObject::matchingRANSAC(std::vector<std:
     }
 
     if (index_best_score != -1)
-        return index_outliers[index_best_score];
+    {
+        std::pair<std::vector<int>, Eigen::Isometry3d> p{index_outliers[index_best_score], best_cl_M_ci};
+        return p;
+    }
     
-    return index_outliers[0];
+
+    std::vector<int> v;
+    std::pair<std::vector<int>, Eigen::Isometry3d> p{v, best_cl_M_ci};
+    
+    return p;
 }
 
-bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen::Isometry3d ci_M_oi, Eigen::Isometry3d cl_M_ci)
+std::ostream &operator<<(std::ostream &flux, Quaterniond quat)
 {
-    double e_pos_th = 0.5;
-    double e_rot_th = 0.5;
+    flux << quat.x() << " ";
+    flux << quat.y() << " ";
+    flux << quat.z() << " ";
+    flux << quat.w() << " ";
 
+    return flux;
+}
+
+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;
@@ -562,8 +586,10 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d cl_M_ol, Eigen:
     quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
     Vector3d pos_feat_identity = identity.translation();
 
+    std::cout << "\n\n\n\n\n" << quat_feat << " " << quat_feat_identity << std::endl;
+    std::cout << pos_feat << "\n" << pos_feat_identity << "\n\n\n\n\n" << std::endl;
 
-    // Error computation
+    // Error between identity and ol_M_oi
     double e_pos = (pos_feat_identity - pos_feat).norm();
     double e_rot = log_q(quat_feat_identity.conjugate() * quat_feat).norm();
 
@@ -595,6 +621,17 @@ void ProcessorTrackerLandmarkObject::resetDerived()
     detections_last_ = std::move(detections_incoming_);
 }
 
+// ostream& operator<<(ostream &flux, Quaterniond quat)
+// {
+//     for (int i = 0; i < quat.size(); i++)
+//     {
+//         flux << quat[i];
+//         flux << " ";
+//     }
+
+//     return flux;
+// }
+
 
 } // namespace wolf
 

test/gtest_processor_tracker_landmark_object.cpp

@@ -10,6 +10,7 @@
 #include "objectslam/feature/feature_object.h"
 #include "objectslam/landmark/landmark_object.h"
 #include "objectslam/internal/config.h"
+#include "objectslam/capture/capture_object.h"
 
 using namespace Eigen;
 using namespace wolf;
@@ -311,6 +312,123 @@ TEST_F(ProcessorTrackerLandmarkObject_class, emplaceFactor)
     ASSERT_TRUE(ctr->getType() == "FactorRelativePose3dWithExtrinsics");
 }
 
+TEST_F(ProcessorTrackerLandmarkObject_class, matchingRANSAC)
+{
+    std::vector<std::tuple<int, int, Eigen::Isometry3d, Eigen::Isometry3d> > last_incoming;
+
+    Vector7d pose_cam_last;
+    Vector7d pose_cam_incoming;
+    Vector7d pose_object_1;
+    Vector7d pose_object_2;
+    Vector7d pose_object_3;
+
+    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;
+    pose_object_3 << 1.2,0.1,3.7,4.5, 2.4,1.4,0.5;
+
+    pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
+    pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
+    pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
+    pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
+    pose_object_3.tail<4>() = pose_object_2.tail<4>().normalized();
+
+
+    Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
+    Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
+
+    Eigen::Isometry3d w_M_o1 = posevec_to_isometry(pose_object_1);
+    Eigen::Isometry3d w_M_o2 = posevec_to_isometry(pose_object_2);
+    Eigen::Isometry3d w_M_o3 = posevec_to_isometry(pose_object_3);
+
+
+    Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
+    Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
+    Eigen::Isometry3d cl_M_o3 = w_M_o3;
+
+    Eigen::Isometry3d ci_M_o1 = w_M_ci.inverse() * w_M_o1;
+    Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2;
+    Eigen::Isometry3d ci_M_o3 = w_M_o3;
+
+    Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
+
+    auto tuple_o1 = std::make_tuple(0, 0, cl_M_o1, ci_M_o1);
+    auto tuple_o2 = std::make_tuple(1, 1, cl_M_o2, ci_M_o2);
+    auto tuple_o3 = std::make_tuple(2, 2, cl_M_o3, ci_M_o3);
+
+    last_incoming.push_back(tuple_o1);
+    last_incoming.push_back(tuple_o2);
+    last_incoming.push_back(tuple_o3);
+
+    auto outliers = ProcessorTrackerLandmarkObject::matchingRANSAC(last_incoming);
+
+    Quaterniond quat_cam;
+    Eigen::Matrix3d wRf = cl_M_ci.linear();
+    quat_cam.coeffs() = R2q(wRf).coeffs().transpose();
+    Vector3d pos_cam = cl_M_ci.translation();
+
+    Quaterniond quat_cam_RANSAC;
+    Eigen::Matrix3d wRf_R = outliers.second.linear();
+    quat_cam_RANSAC.coeffs() = R2q(wRf_R).coeffs().transpose();
+    Vector3d pos_cam_RANSAC = outliers.second.translation();
+
+    std::cout << "\n\n\n\n\n" << outliers.first.size() << "\n\n\n\n";
+
+    std::cout << "\n\n\n\n\n\n\n\n" << pos_cam << "\n" << pos_cam_RANSAC << "\n\n";
+
+    for (int i = 0; i < pos_cam.size(); i++)
+    {
+        ASSERT_TRUE(abs(pos_cam[i] - pos_cam_RANSAC[i]) < 0.001);
+    }
+
+    ASSERT_TRUE(outliers.first.size() == 1);
+    ASSERT_TRUE(outliers.first[0] == 2);
+}
+
+TEST(ProcessorTrackerLandmarkObject, isInliers)
+{
+    Vector7d pose_cam_last;
+    Vector7d pose_cam_incoming;
+    Vector7d pose_object_1;
+    Vector7d pose_object_2;
+
+    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;
+
+    pose_cam_last.tail<4>() = pose_cam_last.tail<4>().normalized();
+    pose_cam_incoming.tail<4>() = pose_cam_incoming.tail<4>().normalized();
+    pose_object_1.tail<4>() = pose_object_1.tail<4>().normalized();
+    pose_object_2.tail<4>() = pose_object_2.tail<4>().normalized();
+
+    Eigen::Isometry3d w_M_cl = posevec_to_isometry(pose_cam_last);
+    Eigen::Isometry3d w_M_ci = posevec_to_isometry(pose_cam_incoming);
+
+    Eigen::Isometry3d w_M_o1 = posevec_to_isometry(pose_object_1);
+    Eigen::Isometry3d w_M_o2 = posevec_to_isometry(pose_object_2);
+    Eigen::Isometry3d w_M_o3 = Eigen::Isometry3d::Identity();
+    
+    
+    Eigen::Isometry3d cl_M_ci = w_M_cl.inverse() * w_M_ci;
+
+    
+    Eigen::Isometry3d cl_M_o1 = w_M_cl.inverse() * w_M_o1;
+    Eigen::Isometry3d cl_M_o2 = w_M_cl.inverse() * w_M_o2;
+    Eigen::Isometry3d cl_M_o3 = w_M_cl.inverse() * w_M_o3;
+    Eigen::Isometry3d ci_M_o1 = w_M_ci.inverse() * w_M_o1;
+    Eigen::Isometry3d ci_M_o2 = w_M_ci.inverse() * w_M_o2;
+    Eigen::Isometry3d ci_M_o3 = w_M_ci.inverse() * w_M_o3;
+
+    ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o1, ci_M_o1, cl_M_ci));
+    ASSERT_TRUE(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o2, cl_M_ci));
+    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)));
+    ASSERT_TRUE(!(ProcessorTrackerLandmarkObject::isInliers(cl_M_o2, ci_M_o1, cl_M_ci)));
+}
+
+
 
 int main(int argc, char **argv)
 {