| 148 | } |
| 149 | |
| 150 | bool LeastAbsoluteDeviationSolver::Solve(const Eigen::VectorXd& b, |
| 151 | Eigen::VectorXd* x) const { |
| 152 | THROW_CHECK_NOTNULL(x); |
| 153 | if (!valid_) { |
| 154 | return false; |
| 155 | } |
| 156 | |
| 157 | Eigen::VectorXd z = Eigen::VectorXd::Zero(A_.rows()); |
| 158 | Eigen::VectorXd z_old(A_.rows()); |
| 159 | Eigen::VectorXd u = Eigen::VectorXd::Zero(A_.rows()); |
| 160 | |
| 161 | Eigen::VectorXd Ax(A_.rows()); |
| 162 | Eigen::VectorXd Ax_hat(A_.rows()); |
| 163 | |
| 164 | const double b_norm = b.norm(); |
| 165 | const double eps_pri_threshold = |
| 166 | std::sqrt(A_.rows()) * options_.absolute_tolerance; |
| 167 | const double eps_dual_threshold = |
| 168 | std::sqrt(A_.cols()) * options_.absolute_tolerance; |
| 169 | |
| 170 | for (int i = 0; i < options_.max_num_iterations; ++i) { |
| 171 | if (!linear_solver_->Solve(A_.transpose() * (b + z - u), x)) { |
| 172 | return false; |
| 173 | } |
| 174 | |
| 175 | Ax.noalias() = A_ * *x; |
| 176 | Ax_hat.noalias() = options_.alpha * Ax + (1 - options_.alpha) * (z + b); |
| 177 | |
| 178 | std::swap(z, z_old); |
| 179 | z.noalias() = Shrinkage(Ax_hat - b + u, 1 / options_.rho); |
| 180 | |
| 181 | u.noalias() += Ax_hat - z - b; |
| 182 | |
| 183 | const double r_norm = (Ax - z - b).norm(); |
| 184 | const double s_norm = (-options_.rho * A_.transpose() * (z - z_old)).norm(); |
| 185 | const double eps_pri = |
| 186 | eps_pri_threshold + options_.relative_tolerance * |
| 187 | std::max(b_norm, std::max(Ax.norm(), z.norm())); |
| 188 | const double eps_dual = |
| 189 | eps_dual_threshold + options_.relative_tolerance * |
| 190 | (options_.rho * A_.transpose() * u).norm(); |
| 191 | |
| 192 | if (r_norm < eps_pri && s_norm < eps_dual) { |
| 193 | break; |
| 194 | } |
| 195 | } |
| 196 | |
| 197 | return true; |
| 198 | } |
| 199 | |
| 200 | } // namespace colmap |