Indentation pbe and simplify isInliers

src/processor/processor_tracker_landmark_object.cpp

@@ -501,41 +501,41 @@ void ProcessorTrackerLandmarkObject::processFeatures(const std::vector<std::pair
                                                      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();
+    {   
+        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);
+        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 feat_last = *feat_last_itr;
+        auto feat_incoming = *feat_incoming_itr;
 
-            auto trackID_feat_last = feat_last->trackId();
+        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);
+        //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);
+            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 
+            if (size_track_feat_last >= 3)
             {
-                //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);
-                
+                //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);
+            
+        }
+    }
 }
 
 void ProcessorTrackerLandmarkObject::filterMatchesOutliers(std::vector<std::pair<int,int> >& _matches, const std::vector<int>& _outliers_idx)
@@ -696,22 +696,12 @@ bool ProcessorTrackerLandmarkObject::isInliers(Eigen::Isometry3d _cl_M_ol, Eigen
     double e_rot_th = 1;
 
     Eigen::Isometry3d ol_M_cl = _cl_M_ol.inverse();
+    // The object is static between last "l" and incoming "i" so this transformation should be close to identity for inliers
     Eigen::Isometry3d ol_M_oi = ol_M_cl * _cl_M_ci * _ci_M_oi;
 
-    Quaterniond quat_feat;
-    Eigen::Matrix3d wRf = ol_M_oi.linear();
-    quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
-    Vector3d pos_feat = ol_M_oi.translation();
-
-    Eigen::Isometry3d identity = Eigen::Isometry3d::Identity();
-    Quaterniond quat_feat_identity;
-    Eigen::Matrix3d wRf_i = identity.linear();
-    quat_feat_identity.coeffs() = R2q(wRf_i).coeffs().transpose();
-    Vector3d pos_feat_identity = identity.translation();
-
     // 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();
+    double e_pos = ol_M_oi.translation().norm();
+    double e_rot = log_R(ol_M_oi.linear()).norm();
 
     return (e_pos < e_pos_th && e_rot < e_rot_th);
 }