diff --git a/xrslam/src/xrslam/backend/backend_worker.cpp b/xrslam/src/xrslam/backend/backend_worker.cpp
index d7d66cd..b8a20aa 100644
--- a/xrslam/src/xrslam/backend/backend_worker.cpp
+++ b/xrslam/src/xrslam/backend/backend_worker.cpp
@@ -67,10 +67,10 @@ void BackendWorker::save_trajectory() {
     FILE *file = fopen(filename.c_str(), "w");
 
     for (const auto &kf : sortedFrames) {
+        Pose pose = kf->get_body_pose();
         fprintf(file, "%.18e %.9e %.9e %.9e %.7e %.7e %.7e %.7e\n", kf->timestamp,
-                kf->pose.p[0], kf->pose.p[1], kf->pose.p[2],
-                kf->pose.q.x(), kf->pose.q.y(), kf->pose.q.z(),
-                kf->pose.q.w());
+                pose.p[0], pose.p[1], pose.p[2],
+                pose.q.x(), pose.q.y(), pose.q.z(), pose.q.w());
         fflush(file);
     } 
     fclose(file);
diff --git a/xrslam/src/xrslam/backend/keyframe.cpp b/xrslam/src/xrslam/backend/keyframe.cpp
index 0a2693e..3266cd5 100644
--- a/xrslam/src/xrslam/backend/keyframe.cpp
+++ b/xrslam/src/xrslam/backend/keyframe.cpp
@@ -15,7 +15,7 @@ namespace xrslam {
 
 KeyFrame::KeyFrame(BackendWorker *backend, Frame *frame)
     : backend(backend), frame_id(frame->id()), timestamp(frame->image->t), 
-    intrinsics(vector<4>(frame->K(0,0), frame->K(1,1), frame->K(0,2), frame->K(1,2))),
+    intrinsics(vector<4>(frame->K(0,0), frame->K(1,1), frame->K(0,2), frame->K(1,2))), camera(frame->camera),
     pose(frame->get_pose(frame->camera)), track_reference_for_frame(0), not_erase(false), to_be_erased(false),
     ba_local_for_keyframe(0), ba_global_for_keyframe(0), ba_fixed_for_keyframe(0), first_connection(true), parent(nullptr) {
     this->tag(KFT_VALID) = true;
@@ -44,6 +44,14 @@ Pose KeyFrame::get_pose(){
     return pose;
 }
 
+Pose KeyFrame::get_body_pose() {
+    std::unique_lock<std::mutex> lock(mutex_pose);
+    Pose body_pose;
+    body_pose.q = pose.q * camera.q_cs.conjugate();
+    body_pose.p = pose.p - this->pose.q * camera.p_cs;
+    return body_pose;
+}
+
 void KeyFrame::update_pose(Pose pose){
     std::unique_lock<std::mutex> lock(mutex_pose);
     this->pose = pose;
@@ -255,6 +263,7 @@ size_t KeyFrame::get_weight(KeyFrame *keyframe) {
 
 void KeyFrame::update_connections() {
     std::map<KeyFrame *, size_t> keyframe_counter;
+    std::map<KeyFrame *, size_t> _connected_keyframe_weights;
 
     for (size_t i = 0; i < features->feature_num(); i++) {
         MapPoint *mp = features->get_mappoint(i);
@@ -294,19 +303,20 @@ void KeyFrame::update_connections() {
         keyframe_max->add_connection(this, n_max);
     }
 
-    std::stable_sort(pairs.begin(), pairs.end());
+    std::sort(pairs.begin(), pairs.end());
     std::list<KeyFrame *> keyframes;
     std::list<size_t> weights;
 
     for (size_t i = 0; i < pairs.size(); i++) {
         keyframes.push_front(pairs[i].second);
         weights.push_front(pairs[i].first);
+        _connected_keyframe_weights.emplace(pairs[i].second, pairs[i].first);
     }
 
     {
         std::unique_lock<std::mutex> lock(mutex_connections);
 
-        connected_keyframe_weights = keyframe_counter;
+        connected_keyframe_weights = _connected_keyframe_weights;
         ordered_connected_keyframes = std::vector<KeyFrame *>(keyframes.begin(), keyframes.end());
         ordered_connected_weights = std::vector<size_t>(weights.begin(), weights.end());
         
@@ -396,7 +406,7 @@ void KeyFrame::update_best_covisibles() {
     for (const auto &it : connected_keyframe_weights)
         pairs.push_back(std::make_pair(it.second, it.first));
 
-    std::stable_sort(pairs.begin(), pairs.end());
+    std::sort(pairs.begin(), pairs.end());
  
     std::list<KeyFrame *> keyframes;
     std::list<size_t> weights;
@@ -526,8 +536,8 @@ float KeyFrame::compute_scene_median_depth() {
         }
     }
     
-    std::stable_sort(depths.begin(), depths.end());
-    return depths[depths.size() / 2];
+    std::sort(depths.begin(), depths.end());
+    return depths[(depths.size()-1) / 2];
 }
 
 int KeyFrame::tracked_mappoints(int min_obs) {
diff --git a/xrslam/src/xrslam/backend/keyframe.h b/xrslam/src/xrslam/backend/keyframe.h
index 05c14ea..bf3e3c8 100644
--- a/xrslam/src/xrslam/backend/keyframe.h
+++ b/xrslam/src/xrslam/backend/keyframe.h
@@ -27,6 +27,7 @@ class KeyFrame: public Tagged<KeyFrameTag> {
     ~KeyFrame();
     
     Pose get_pose();
+    Pose get_body_pose();
     void update_pose(Pose pose);
     void set_invalid();
     void compute_bow();
@@ -74,6 +75,7 @@ class KeyFrame: public Tagged<KeyFrameTag> {
     const size_t frame_id;
     const double timestamp;
     vector<4> intrinsics;
+    ExtrinsicParams camera;
     size_t width;
     size_t height;
     size_t track_reference_for_frame;
diff --git a/xrslam/src/xrslam/backend/loop_closer.cpp b/xrslam/src/xrslam/backend/loop_closer.cpp
index 8eb11f1..123c80d 100644
--- a/xrslam/src/xrslam/backend/loop_closer.cpp
+++ b/xrslam/src/xrslam/backend/loop_closer.cpp
@@ -103,7 +103,7 @@ bool LoopCloser::detect_loop() {
     std::vector<bool> consistent_group(consistent_groups.size(), false);
 
     for(const auto &keyframe : candidate_keyframes) {
-        std::set<KeyFrame *> candidate_group =  keyframe->get_connected_keyframes();
+        std::set<KeyFrame *> candidate_group = keyframe->get_connected_keyframes();
         candidate_group.insert(keyframe);
 
         bool enough_consistent = false;
@@ -115,6 +115,7 @@ bool LoopCloser::detect_loop() {
             for(const auto &kf : candidate_group) {
                 if(group.first.count(kf)) {
                     consistent = true;
+                    consistent_for_some_group = true;
                     break;
                 }
             }
diff --git a/xrslam/src/xrslam/backend/map_point.cpp b/xrslam/src/xrslam/backend/map_point.cpp
index ad0f1a9..f5f0b81 100644
--- a/xrslam/src/xrslam/backend/map_point.cpp
+++ b/xrslam/src/xrslam/backend/map_point.cpp
@@ -134,11 +134,10 @@ void MapPoint::compute_distinctive_descriptors() {
         
     std::vector<cv::Mat> descriptors;
     descriptors.reserve(_observations.size());
-    for (std::map<KeyFrame *, size_t>::iterator 
-            mit=_observations.begin(); mit!=_observations.end(); mit++) {
-        KeyFrame *kf = mit->first;
+    for (const auto &observation : _observations) {
+        KeyFrame *kf = observation.first;
         if(kf->tag(KFT_VALID)) {
-            descriptors.push_back(kf->get_features()->descriptors.row(mit->second));
+            descriptors.push_back(kf->get_features()->descriptors.row(observation.second));
         }
     }
 
@@ -150,7 +149,7 @@ void MapPoint::compute_distinctive_descriptors() {
     distances.resize(N, std::vector<float>(N, 0));
     for (size_t i = 0; i < N; i++) {
         distances[i][i] = 0;
-        for (size_t j = 0; j < N; j++) {
+        for (size_t j = i+1; j < N; j++) {
             int dist_ij = ORBmatcher::descriptor_distance(descriptors[i], descriptors[j]);
             distances[i][j] = dist_ij;
             distances[j][i] = dist_ij;
@@ -190,22 +189,22 @@ void MapPoint::update_normal_and_depth() {
 
     int n = 0;
     vector<3> _normal = vector<3>(0,0,0);
-    for (std::map<KeyFrame *, size_t>::iterator mit=_observations.begin(); mit!=_observations.end(); mit++) {
-        KeyFrame *keyframe = mit->first;
-        Pose pose = keyframe->get_pose();
-        vector<3> normali = landmark - pose.p;
+    for (const auto &observation : _observations) {
+        KeyFrame *keyframe = observation.first;
+        vector<3> twc = keyframe->get_camera_to_world_translation();
+        vector<3> normali = landmark - twc;
         _normal = _normal + normali/normali.norm();
         n++;
     }
 
-    Pose ref_pose = ref_keyframe->get_pose();
-    vector<3> pc = _landmark - ref_pose.p;
+    vector<3> twc_ref = ref_keyframe->get_camera_to_world_translation();
+    vector<3> pc = _landmark - twc_ref;
     float dis = pc.norm();
 
     auto ref_features = ref_keyframe->get_features();
 
     int level = ref_features->cvkeypoints[_observations[ref_keyframe]].octave;
-    float level_scale_factor =  ref_features->scale_factors[level];
+    float level_scale_factor = ref_features->scale_factors[level];
     int levels = ref_features->scale_levels; 
 
     {
@@ -253,12 +252,12 @@ cv::Mat MapPoint::get_descriptor() {
 
 float MapPoint::get_min_distance_invariance() {
     std::unique_lock<std::mutex> lock(mutex_landmark);
-    return 0.8 * min_distance;
+    return 0.6 * min_distance;
 }
 
 float MapPoint::get_max_distance_invariance() {
     std::unique_lock<std::mutex> lock(mutex_landmark);
-    return 1.2 * max_distance;
+    return 1.5 * max_distance;
 }
 
 void MapPoint::increase_visible(size_t n) {
diff --git a/xrslam/src/xrslam/backend/orb_matcher.cpp b/xrslam/src/xrslam/backend/orb_matcher.cpp
index 07a9724..b4fe102 100644
--- a/xrslam/src/xrslam/backend/orb_matcher.cpp
+++ b/xrslam/src/xrslam/backend/orb_matcher.cpp
@@ -351,7 +351,7 @@ int ORBmatcher::search_by_projection(KeyFrame *keyframe, const std::set<MapPoint
         int best_level = -1;
         int best_dist2 = 256;
         int best_level2 = -1;
-        int best_index =-1 ;
+        int best_index = -1;
 
         for (const auto &i : indices) {
             MapPoint *mp = features->get_mappoint(i);
@@ -367,7 +367,7 @@ int ORBmatcher::search_by_projection(KeyFrame *keyframe, const std::set<MapPoint
                 best_dist = dist;
                 best_level2 = best_level;
                 best_level = features->cvkeypoints[i].octave;
-                best_index=i;
+                best_index = i;
             }
             else if(dist < best_dist2) {
                 best_dist2 = dist;
@@ -730,7 +730,7 @@ int ORBmatcher::search_for_triangulation(KeyFrame *keyframe1, KeyFrame *keyframe
                         continue;
 
                     const cv::Mat &d2 = features2->descriptors.row(index2);
-                    int dist = descriptor_distance(d1,d2);
+                    int dist = descriptor_distance(d1, d2);
                     
                     if(dist > TH_LOW || dist > best_dist)
                         continue;
@@ -877,8 +877,6 @@ int ORBmatcher::fuse(KeyFrame *keyframe, std::vector<MapPoint *> &mappoints, con
     const float &cy = keyframe->intrinsics(3);
 
     auto features = keyframe->get_features();
-    const quaternion qwc = keyframe->get_camera_to_world_rotation();
-    const vector<3> twc = keyframe->get_camera_to_world_translation();
 
     for(int i = 0; i < mappoints.size(); i++) {
         MapPoint* mp = mappoints[i];
@@ -889,7 +887,7 @@ int ORBmatcher::fuse(KeyFrame *keyframe, std::vector<MapPoint *> &mappoints, con
             continue;
 
         vector<3> Pw = mp->get_landmark();
-        vector<3> Pc = qwc.conjugate() * (Pw - twc);
+        vector<3> Pc = keyframe->world_to_camera(Pw);
 
         const float x = Pc(0);
         const float y = Pc(1);
@@ -907,6 +905,7 @@ int ORBmatcher::fuse(KeyFrame *keyframe, std::vector<MapPoint *> &mappoints, con
         const float max_distance = mp->get_max_distance_invariance();
         const float min_distance = mp->get_min_distance_invariance();
 
+        vector<3> twc = keyframe->get_camera_to_world_translation();
         vector<3> PO = Pw - twc;
         float dist = PO.norm();
 
@@ -964,7 +963,7 @@ int ORBmatcher::fuse(KeyFrame *keyframe, std::vector<MapPoint *> &mappoints, con
             MapPoint *mp_kf = features->get_mappoint(best_index);
             if(mp_kf)
             {
-                if(!mp_kf->tag(MPT_VALID)) {
+                if(mp_kf->tag(MPT_VALID)) {
                     if(mp_kf->observation_num() > mp->observation_num())
                         mp->replace(mp_kf);
                     else
diff --git a/xrslam/src/xrslam/backend/orb_matcher.h b/xrslam/src/xrslam/backend/orb_matcher.h
index 86d5ea2..79b2558 100644
--- a/xrslam/src/xrslam/backend/orb_matcher.h
+++ b/xrslam/src/xrslam/backend/orb_matcher.h
@@ -38,7 +38,7 @@ class ORBmatcher {
 
     void compute_three_maxima(std::vector<int>* histo, const int L, int &ind1, int &ind2, int &ind3);
 
-    int fuse(KeyFrame *keyframe, std::vector<MapPoint *> &mappoints, const float th=3.0);
+    int fuse(KeyFrame *keyframe, std::vector<MapPoint *> &mappoints, const float th=5.0);
 
     int fuse(KeyFrame *keyframe, Pose pose, std::vector<MapPoint *> &loop_mappoints, std::vector<MapPoint *> &replace_mappoints, const float th=3.0);