diff --git a/src/stitch/camera_estimator.cc b/src/stitch/camera_estimator.cc
index 88a883d1..480f3137 100644
--- a/src/stitch/camera_estimator.cc
+++ b/src/stitch/camera_estimator.cc
@@ -1,5 +1,5 @@
 //File: camera_estimator.cc
-//Date:
+//Date: Wed May 03 05:20:02 2017 -0700
 //Author: Yuxin Wu <ppwwyyxxc@gmail.com>
 
 #include "camera_estimator.hh"
@@ -54,6 +54,7 @@ vector<Camera> CameraEstimator::estimate() {
         // set the starting point to identity
         cameras[node].R = Homography::I();
         cameras[node].ppx = cameras[node].ppy = 0;
+        iba.set_identity_idx(node);
       },
       [&](int now, int next) {
         print_debug("Best edge from %d to %d\n", now, next);
diff --git a/src/stitch/incremental_bundle_adjuster.cc b/src/stitch/incremental_bundle_adjuster.cc
index 75ecd9be..566b379f 100644
--- a/src/stitch/incremental_bundle_adjuster.cc
+++ b/src/stitch/incremental_bundle_adjuster.cc
@@ -135,13 +135,17 @@ void IncrementalBundleAdjuster::optimize() {
   int itr = 0;
   int nr_non_decrease = 0;// number of non-decreasing iteration
   inlier_threshold = std::numeric_limits<int>::max();
+  size_t idt = index_map[identity_idx];
   while (itr++ < LM_MAX_ITER) {
     auto update = get_param_update(state, err_stat.residuals, LM_LAMBDA);
 
     ParamState new_state;
     new_state.params = state.get_params();
-    REP(i, new_state.params.size())
-      new_state.params[i] -= update(i);
+    REP(i, new_state.params.size()) {
+      // 3~6 of parameters is R
+      if (i < idt * 6 + 3 or i >= idt * 6 + 6)  // do not update R of identity image
+        new_state.params[i] -= update(i);
+    }
     err_stat = calcError(new_state);
     print_debug("BA: average err: %lf, max: %lf\n", err_stat.avg, err_stat.max);
 
@@ -159,6 +163,7 @@ void IncrementalBundleAdjuster::optimize() {
 
   auto results = state.get_cameras();
   int now = 0;
+  // idx_added is sorted
   for (auto& i : idx_added)
     result_cameras[i] = results[now++];
 }
diff --git a/src/stitch/incremental_bundle_adjuster.hh b/src/stitch/incremental_bundle_adjuster.hh
index f55f5a4b..9c6d4b41 100644
--- a/src/stitch/incremental_bundle_adjuster.hh
+++ b/src/stitch/incremental_bundle_adjuster.hh
@@ -1,5 +1,5 @@
 //File: incremental_bundle_adjuster.hh
-//Date:
+//Date: Wed May 03 05:19:14 2017 -0700
 //Author: Yuxin Wu <ppwwyyxxc@gmail.com>
 
 #pragma once
@@ -49,6 +49,8 @@ class IncrementalBundleAdjuster {
 			return calcError(state);
 		}
 
+    void set_identity_idx(int idx) { identity_idx = idx; }
+
 	protected:
 		std::vector<Camera>& result_cameras;
 
@@ -63,7 +65,11 @@ class IncrementalBundleAdjuster {
 		int inlier_threshold = std::numeric_limits<int>::max();
 		std::vector<MatchPair> match_pairs;
 
-		// original indices that have appeared so far
+    // original idx of the identity image
+    int identity_idx = -1;
+		// Original indices that have appeared so far.
+    // Sorted so that in the end can output directly to
+    // the camera vector of original indices
 		std::set<int> idx_added;
 
 		// map from original image index to index being added
diff --git a/src/stitch/stitcher.cc b/src/stitch/stitcher.cc
index 1d6a60b8..c7b9d54a 100644
--- a/src/stitch/stitcher.cc
+++ b/src/stitch/stitcher.cc
@@ -1,5 +1,5 @@
 // File: stitcher.cc
-// Date: Sun Sep 22 12:54:18 2013 +0800
+// Date: Wed May 03 05:25:53 2017 -0700
 // Author: Yuxin Wu <ppwwyyxxc@gmail.com>
 
 
@@ -59,6 +59,7 @@ Mat32f Stitcher::build() {
     bundle.proj_method = ConnectedImages::ProjectionMethod::flat;
   print_debug("Using projection method: %d\n", bundle.proj_method);
   bundle.update_proj_range();
+
   return bundle.blend();
 }
 
@@ -146,6 +147,7 @@ void Stitcher::estimate_camera() {
 
   // produced homo operates on [-w/2,w/2] coordinate
   REP(i, imgs.size()) {
+    //cout << "Camera " << i << " " << cameras[i].R << ", " << cameras[i].K() << endl;
     bundle.component[i].homo_inv = cameras[i].K() * cameras[i].R;
     bundle.component[i].homo = cameras[i].Rinv() * cameras[i].K().inverse();
   }
diff --git a/src/stitch/stitcher_image.hh b/src/stitch/stitcher_image.hh
index b29d03c8..c2894606 100644
--- a/src/stitch/stitcher_image.hh
+++ b/src/stitch/stitcher_image.hh
@@ -37,7 +37,7 @@ struct ConnectedImages {
 
 	struct ImageComponent {
 		Homography homo,			// from me (point in image plane, 2d) to point in space (P,3D)
-							 homo_inv;	// transform from point in space to me (point in plane):
+							 homo_inv;	// transform from point in space (P) to me (point in plane):
                           // p = K * R * P
 
 		// point to the original image