From baab7fcfc0cba6b278f0159f84dffa800f6b22d5 Mon Sep 17 00:00:00 2001
From: Peihong Guo <peihongguo@gmail.com>
Date: Thu, 4 Feb 2016 23:28:08 -0600
Subject: [PATCH] Multi image reconstruction with subset selection finished.

---
 multiimagereconstructor.h | 43 +++++++++++++++++++++++++++++++--------
 statsutils.h              | 28 +++++++++++++++++++++++--
 2 files changed, 60 insertions(+), 11 deletions(-)

diff --git a/multiimagereconstructor.h b/multiimagereconstructor.h
index 6278fb7..d5e3c3d 100644
--- a/multiimagereconstructor.h
+++ b/multiimagereconstructor.h
@@ -149,10 +149,13 @@ bool MultiImageReconstructor<Constraint>::Reconstruct() {
   vector<MatrixXd> identity_weights_history;
   vector<VectorXd> identity_weights_centroid_history;
 
+  vector<int> consistent_set(num_images);
+  for(int i=0;i<num_images;++i) consistent_set[i] = i;
+
   while(iters_main_loop++ < 3){
 
     // Single image reconstruction step
-    for(size_t i=0;i<num_images;++i) {
+    for(int i=0;i<num_images;++i) {
       single_recon.SetMesh(param_sets[i].mesh);
       single_recon.SetIndices(param_sets[i].indices);
       single_recon.SetImageSize(param_sets[i].recon.imageWidth, param_sets[i].recon.imageHeight);
@@ -199,15 +202,23 @@ bool MultiImageReconstructor<Constraint>::Reconstruct() {
     identity_weights_history.push_back(identity_weights);
 
     // Remove outliers
-    vector<int> consistent_set = StatsUtils::FindConsistentSet(identity_weights, 0.5);
+    #if 1
+    const double ratios[] = {0.0, 0.2, 0.4, 0.6};
+    consistent_set = StatsUtils::FindConsistentSet(identity_weights, 0.5, ratios[iters_main_loop] * num_images, &identity_centroid);
     assert(consistent_set.size() > 0);
-
+    for(auto i : consistent_set) {
+      cout << i << ' ';
+    }
+    cout << endl;
+    #else
     // Compute the centroid of the consistent set
     identity_centroid = VectorXd::Zero(param_sets[0].model.Wid.rows());
     for(auto i : consistent_set) {
+      cout << i << endl;
       identity_centroid += param_sets[i].model.Wid;
     }
     identity_centroid /= consistent_set.size();
+    #endif
 
     // Update the identity weights for all images
     for(auto& param : param_sets) {
@@ -215,10 +226,19 @@ bool MultiImageReconstructor<Constraint>::Reconstruct() {
     }
 
     // Joint reconstruction step, obtain refined identity weights
-    const int num_iters_joint_optimization = 3;
-    for(int i=0;i<num_iters_joint_optimization; ++i){
+    int num_iters_joint_optimization = (iters_main_loop == max_iters_main_loop)?4:3;
+
+    for(int iters_joint_optimization=0;
+        iters_joint_optimization<num_iters_joint_optimization;
+        ++iters_joint_optimization){
       // [Joint reconstruction] step 1: estimate pose and expression weights individually
-      for(size_t i=0;i<num_images;++i) {
+      if((iters_joint_optimization == num_iters_joint_optimization - 1) && (iters_main_loop == max_iters_main_loop)) {
+        // In the final iteration, no need to refine the identity weights anymore
+        consistent_set.resize(num_images);
+        for(int i=0;i<num_images;++i) consistent_set[i] = i;
+      }
+
+      for(auto i : consistent_set) {
         single_recon.SetMesh(param_sets[i].mesh);
         single_recon.SetIndices(param_sets[i].indices);
         single_recon.SetImageSize(param_sets[i].recon.imageWidth, param_sets[i].recon.imageHeight);
@@ -258,13 +278,18 @@ bool MultiImageReconstructor<Constraint>::Reconstruct() {
         }
       }
 
+      if((iters_joint_optimization == num_iters_joint_optimization - 1) && (iters_main_loop == max_iters_main_loop)) {
+        // In the final iteration, no need to refine the identity weights anymore
+        break;
+      }
+
       // [Joint reconstruction] step 2: estimate identity weights jointly
       {
         ceres::Problem problem;
         VectorXd params = param_sets[0].model.Wid;
 
         // Add constraints from each image
-        for(int i=0;i<num_images;++i) {
+        for(auto i : consistent_set) {
           // Create a projected model first
           vector<MultilinearModel> model_projected_i(param_sets[i].indices.size());
           for(int j=0;j<param_sets[i].indices.size();++j) {
@@ -300,7 +325,7 @@ bool MultiImageReconstructor<Constraint>::Reconstruct() {
         ceres::DynamicNumericDiffCostFunction<PriorCostFunction> *prior_cost_function =
           new ceres::DynamicNumericDiffCostFunction<PriorCostFunction>(
             new PriorCostFunction(prior.Wid_avg, prior.inv_sigma_Wid,
-                                  prior.weight_Wid * num_images));
+                                  prior.weight_Wid * consistent_set.size()));
         prior_cost_function->AddParameterBlock(params.size());
         prior_cost_function->SetNumResiduals(1);
         problem.AddResidualBlock(prior_cost_function, NULL, params.data());
@@ -352,7 +377,7 @@ bool MultiImageReconstructor<Constraint>::Reconstruct() {
   }
 
   // Visualize the final reconstruction results
-  for(size_t i=0;i<num_images;++i) {
+  for(int i=0;i<num_images;++i) {
     // Visualize the reconstruction results
     MeshVisualizer* w = new MeshVisualizer("reconstruction result", param_sets[i].mesh);
     w->BindConstraints(image_points_pairs[i].second);
diff --git a/statsutils.h b/statsutils.h
index 21cdc3f..795f6ff 100644
--- a/statsutils.h
+++ b/statsutils.h
@@ -78,7 +78,7 @@ static vector<int> FindConsistentSet(const MatrixXd& x, double h, int k,
   while(!done && iters < max_iters) {
     for(int i=0;i<nsamples;++i) {
       double gsum = 0;
-      VectorXd yi = VectorXd::Zeros(ndims);
+      VectorXd yi = VectorXd::Zero(ndims);
       for(int j=0;j<nsamples;++j) {
         if(j==i) continue;
         else {
@@ -107,17 +107,41 @@ static vector<int> FindConsistentSet(const MatrixXd& x, double h, int k,
   vector<double> d(nsamples, 0);
   for(int i=0;i<nsamples;++i) {
     for(int j=0;j<nsamples;++j) {
-      d[i] += exp(-(m.col(i) - m.col(j)).squaredNorm());
+      d[i] += exp(-(y.col(i) - y.col(j)).squaredNorm());
     }
   }
 
+  double max_d = 0;
+  int max_idx = -1;
+  for(int i=0;i<nsamples;++i) {
+    if(d[i] > max_d) {
+      max_d = d[i];
+      max_idx = i;
+    }
+  }
+
+  VectorXd centroid = y.col(max_idx);
   if(centroid_out != nullptr) {
     // Write the centroid to the output
+    *centroid_out = centroid;
   }
 
   // Compute the distance between the centroid and each input point
+  vector<pair<int, double>> dists(nsamples);
+  for(int i=0;i<nsamples;++i) {
+    dists[i] = make_pair(i, (y.col(i) - centroid).norm());
+  }
+
+  std::sort(dists.begin(), dists.end(),
+            [](const pair<int, double>& a, const pair<int, double>& b) {
+              return a.second < b.second;
+            });
 
   // Pick the k nearest points as the consistent set
+  vector<int> consistent_set;
+  for(int i=0;i<k;++i) {
+    consistent_set.push_back(dists[i].first);
+  }
 
   return consistent_set;
 }