Robust weight estimation

muskie82 · Oct 16, 2018 · b9cf676 · b9cf676
1 parent 967080e
commit b9cf676
Show file tree

Hide file tree

Showing 2 changed files with 50 additions and 0 deletions.
diff --git a/include/image_alignment.h b/include/image_alignment.h
@@ -33,6 +33,10 @@ class DirectImageAlignment {
     cv::Mat depth_cur_Pyramid[num_pyramid];
     Eigen::Matrix3f k_Pyramid[num_pyramid];
 
+    //Robust weight estimation
+    static constexpr float INITIAL_SIGMA = 5.0f;
+    static constexpr float DEFAULT_DOF = 5.0f;
+
 public:
 
     /**
@@ -66,6 +70,11 @@ class DirectImageAlignment {
      */
     Eigen::MatrixXf calcJacobian(const Eigen::VectorXf &xi, const int level);
 
+    /**
+     * @brief Compute robust weights from residuals.
+     */
+    void weighting(Eigen::VectorXf &residuals, Eigen::VectorXf &weights);
+
     /**
      * @brief Execute GaussNewton optimzation and find the optimam rotation and translation.
      * @param[out] rot, t: camera pose transformation between 2 images.

diff --git a/src/image_alignment.cpp b/src/image_alignment.cpp
@@ -276,6 +276,37 @@ Eigen::MatrixXf DirectImageAlignment::calcJacobian(const Eigen::VectorXf &xi, co
     return J;
 }
 
+void  DirectImageAlignment::weighting(Eigen::VectorXf &residuals, Eigen::VectorXf &weights) {
+    int n = residuals.size();
+    float lambda_init = 1.0f / (INITIAL_SIGMA * INITIAL_SIGMA);
+    float lambda = lambda_init;
+    float num = 0.0;
+    float dof = DEFAULT_DOF;
+    weights = Eigen::VectorXf::Ones(n);
+    int itr = 0;
+    do {
+        itr++;
+        lambda_init = lambda;
+        lambda = 0.0f;
+        num = 0.0f;
+        for(int i = 0; i < n; ++i) {
+            float data = residuals(i);
+
+            if(std::isfinite(data)) {
+                num += 1.0f;
+                lambda += data * data * ( (dof + 1.0f) / (dof + lambda_init * data * data) );
+            }
+        }
+        lambda /= num;
+        lambda = 1.0f / lambda;
+    } while(std::abs(lambda - lambda_init) > 1e-3);
+
+    for(int i=0; i<n; i++){
+        float data = residuals(i);
+        weights(i) = ( (dof + 1.0f) / (dof + lambda * data * data) );
+    }
+}
+
 void DirectImageAlignment::doGaussNewton(Eigen::Matrix3f& rot, Eigen::Vector3f& t){
 
     Eigen::VectorXf xi, xi_prev;
@@ -290,8 +321,18 @@ void DirectImageAlignment::doGaussNewton(Eigen::Matrix3f& rot, Eigen::Vector3f&
         for (int itr = 0; itr < num_GNiterations; itr++) {
             // compute residuals and Jacobian
             Eigen::VectorXf residuals = calcRes(xi, level);
+            Eigen::VectorXf weights;
+
+            weighting(residuals, weights);
+            residuals = residuals.cwiseProduct(weights);
+
             Eigen::MatrixXf J = calcJacobian(xi, level);
+            // compute weighted Jacobian
+            for (int i = 0; i < residuals.size(); ++i)
+                for (int j = 0; j < J.cols(); ++j)
+                    J(i, j) = J(i, j) * weights[i];
             Eigen::MatrixXf Jt = J.transpose();
+
             float error = residuals.transpose()*residuals;
 
             // compute update step.