void optimizeGaussNewton( const double reproj_thresh, const size_t n_iter, const bool verbose, FramePtr& frame, double& estimated_scale, double& error_init, double& error_final, size_t& num_obs) { // init double chi2(0.0); vector<double> chi2_vec_init, chi2_vec_final; vk::robust_cost::TukeyWeightFunction weight_function; SE3d T_old(frame->T_f_w_); Matrix6d A; Vector6d b; // compute the scale of the error for robust estimation std::vector<float> errors; errors.reserve(frame->fts_.size()); for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) { if((*it)->point == NULL) continue; Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); e *= 1.0 / (1<<(*it)->level); errors.push_back(e.norm()); } if(errors.empty()) return; vk::robust_cost::MADScaleEstimator scale_estimator; estimated_scale = scale_estimator.compute(errors); num_obs = errors.size(); chi2_vec_init.reserve(num_obs); chi2_vec_final.reserve(num_obs); double scale = estimated_scale; for(size_t iter=0; iter<n_iter; iter++) { // overwrite scale if(iter == 5) scale = 0.85/frame->cam_->errorMultiplier2(); b.setZero(); A.setZero(); double new_chi2(0.0); // compute residual for(auto it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) { if((*it)->point == NULL) continue; Matrix26d J; Vector3d xyz_f(frame->T_f_w_ * (*it)->point->pos_); Frame::jacobian_xyz2uv(xyz_f, J); Vector2d e = vk::project2d((*it)->f) - vk::project2d(xyz_f); double sqrt_inv_cov = 1.0 / (1<<(*it)->level); e *= sqrt_inv_cov; if(iter == 0) chi2_vec_init.push_back(e.squaredNorm()); // just for debug J *= sqrt_inv_cov; double weight = weight_function.value(e.norm()/scale); A.noalias() += J.transpose()*J*weight; b.noalias() -= J.transpose()*e*weight; new_chi2 += e.squaredNorm()*weight; } // solve linear system const Vector6d dT(A.ldlt().solve(b)); // check if error increased if((iter > 0 && new_chi2 > chi2) || (bool) std::isnan((double)dT[0])) { if(verbose) std::cout << "it " << iter << "\t FAILURE \t new_chi2 = " << new_chi2 << std::endl; frame->T_f_w_ = T_old; // roll-back break; } // update the model SE3d T_new = SE3d::exp(dT)*frame->T_f_w_; T_old = frame->T_f_w_; frame->T_f_w_ = T_new; chi2 = new_chi2; if(verbose) std::cout << "it " << iter << "\t Success \t new_chi2 = " << new_chi2 << "\t norm(dT) = " << vk::norm_max(dT) << std::endl; // stop when converged if(vk::norm_max(dT) <= EPS) break; } // Set covariance as inverse information matrix. Optimistic estimator! const double pixel_variance=1.0; frame->Cov_ = pixel_variance*(A*std::pow(frame->cam_->errorMultiplier2(),2)).inverse(); // Remove Measurements with too large reprojection error double reproj_thresh_scaled = reproj_thresh / frame->cam_->errorMultiplier2(); size_t n_deleted_refs = 0; for(Features::iterator it=frame->fts_.begin(); it!=frame->fts_.end(); ++it) { if((*it)->point == NULL) continue; Vector2d e = vk::project2d((*it)->f) - vk::project2d(frame->T_f_w_ * (*it)->point->pos_); double sqrt_inv_cov = 1.0 / (1<<(*it)->level); e *= sqrt_inv_cov; chi2_vec_final.push_back(e.squaredNorm()); if(e.norm() > reproj_thresh_scaled) { // we don't need to delete a reference in the point since it was not created yet (*it)->point = NULL; ++n_deleted_refs; } } error_init=0.0; error_final=0.0; if(!chi2_vec_init.empty()) error_init = sqrt(vk::getMedian(chi2_vec_init))*frame->cam_->errorMultiplier2(); if(!chi2_vec_final.empty()) error_final = sqrt(vk::getMedian(chi2_vec_final))*frame->cam_->errorMultiplier2(); estimated_scale *= frame->cam_->errorMultiplier2(); if(verbose) std::cout << "n deleted obs = " << n_deleted_refs << "\t scale = " << estimated_scale << "\t error init = " << error_init << "\t error end = " << error_final << std::endl; num_obs -= n_deleted_refs; }
bool DenseTracker::match(dvo::core::PointSelection& reference, dvo::core::RgbdImagePyramid& current, dvo::DenseTracker::Result& result) { current.compute(cfg.getNumLevels()); bool success = true; if(cfg.UseInitialEstimate) { assert(!result.isNaN() && "Provided initialization is NaN!"); } else { result.setIdentity(); } // our first increment is the given guess Sophus::SE3d inc(result.Transformation.rotation(), result.Transformation.translation()); Revertable<Sophus::SE3d> initial(inc); Revertable<Sophus::SE3d> estimate; bool accept = true; //static stopwatch_collection sw_level(5, "l", 100); //static stopwatch_collection sw_it(5, "it@l", 500); //static stopwatch_collection sw_error(5, "err@l", 500); //static stopwatch_collection sw_linsys(5, "linsys@l", 500); //static stopwatch_collection sw_prep(5, "prep@l", 100); if(points_error.size() < reference.getMaximumNumberOfPoints(cfg.LastLevel)) points_error.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel)); if(residuals.size() < reference.getMaximumNumberOfPoints(cfg.LastLevel)) residuals.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel)); if(weights.size() < reference.getMaximumNumberOfPoints(cfg.LastLevel)) weights.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel)); std::vector<uint8_t> valid_residuals; bool debug = false; if(debug) { reference.debug(true); valid_residuals.resize(reference.getMaximumNumberOfPoints(cfg.LastLevel)); } /* std::stringstream name; name << std::setiosflags(std::ios::fixed) << std::setprecision(2) << current.timestamp() << "_error.avi"; cv::Size s = reference.getRgbdImagePyramid().level(size_t(cfg.LastLevel)).intensity.size(); cv::Mat video_frame(s.height, s.width * 2, CV_32FC1), video_frame_u8; cv::VideoWriter vw(name.str(), CV_FOURCC('P','I','M','1'), 30, video_frame.size(), false); float rgb_max = 0.0; float depth_max = 0.0; std::stringstream name1; name1 << std::setiosflags(std::ios::fixed) << std::setprecision(2) << current.timestamp() << "_ref.png"; cv::imwrite(name1.str(), current.level(0).rgb); std::stringstream name2; name2 << std::setiosflags(std::ios::fixed) << std::setprecision(2) << current.timestamp() << "_cur.png"; cv::imwrite(name2.str(), reference.getRgbdImagePyramid().level(0).rgb); */ Eigen::Vector2f mean; mean.setZero(); Eigen::Matrix2f /*first_precision,*/ precision; precision.setZero(); for(itctx_.Level = cfg.FirstLevel; itctx_.Level >= cfg.LastLevel; --itctx_.Level) { result.Statistics.Levels.push_back(LevelStats()); LevelStats& level_stats = result.Statistics.Levels.back(); mean.setZero(); precision.setZero(); // reset error after every pyramid level? yes because errors from different levels are not comparable itctx_.Iteration = 0; itctx_.Error = std::numeric_limits<double>::max(); RgbdImage& cur = current.level(itctx_.Level); const IntrinsicMatrix& K = cur.camera().intrinsics(); Vector8f wcur, wref; // i z idx idy zdx zdy float wcur_id = 0.5f, wref_id = 0.5f, wcur_zd = 1.0f, wref_zd = 0.0f; wcur << 1.0f / 255.0f, 1.0f, wcur_id * K.fx() / 255.0f, wcur_id * K.fy() / 255.0f, wcur_zd * K.fx(), wcur_zd * K.fy(), 0.0f, 0.0f; wref << -1.0f / 255.0f, -1.0f, wref_id * K.fx() / 255.0f, wref_id * K.fy() / 255.0f, wref_zd * K.fx(), wref_zd * K.fy(), 0.0f, 0.0f; // sw_prep[itctx_.Level].start(); PointSelection::PointIterator first_point, last_point; reference.select(itctx_.Level, first_point, last_point); cur.buildAccelerationStructure(); level_stats.Id = itctx_.Level; level_stats.MaxValidPixels = reference.getMaximumNumberOfPoints(itctx_.Level); level_stats.ValidPixels = last_point - first_point; // sw_prep[itctx_.Level].stopAndPrint(); NormalEquationsLeastSquares ls; Matrix6d A; Vector6d x, b; x = inc.log(); ComputeResidualsResult compute_residuals_result; compute_residuals_result.first_point_error = points_error.begin(); compute_residuals_result.first_residual = residuals.begin(); compute_residuals_result.first_valid_flag = valid_residuals.begin(); // sw_level[itctx_.Level].start(); do { level_stats.Iterations.push_back(IterationStats()); IterationStats& iteration_stats = level_stats.Iterations.back(); iteration_stats.Id = itctx_.Iteration; // sw_it[itctx_.Level].start(); double total_error = 0.0f; // sw_error[itctx_.Level].start(); Eigen::Affine3f transformf; inc = Sophus::SE3d::exp(x); initial.update() = inc.inverse() * initial(); estimate.update() = inc * estimate(); transformf = estimate().matrix().cast<float>(); if(debug) { dvo::core::computeResidualsAndValidFlagsSse(first_point, last_point, cur, K, transformf, wref, wcur, compute_residuals_result); } else { dvo::core::computeResidualsSse(first_point, last_point, cur, K, transformf, wref, wcur, compute_residuals_result); } size_t n = (compute_residuals_result.last_residual - compute_residuals_result.first_residual); iteration_stats.ValidConstraints = n; if(n < 6) { initial.revert(); estimate.revert(); level_stats.TerminationCriterion = TerminationCriteria::TooFewConstraints; break; } if(itctx_.IsFirstIterationOnLevel()) { std::fill(weights.begin(), weights.begin() + n, 1.0f); } else { dvo::core::computeWeightsSse(compute_residuals_result.first_residual, compute_residuals_result.last_residual, weights.begin(), mean, precision); } precision = dvo::core::computeScaleSse(compute_residuals_result.first_residual, compute_residuals_result.last_residual, weights.begin(), mean).inverse(); float ll = computeCompleteDataLogLikelihood(compute_residuals_result.first_residual, compute_residuals_result.last_residual, weights.begin(), mean, precision); iteration_stats.TDistributionLogLikelihood = -ll; iteration_stats.TDistributionMean = mean.cast<double>(); iteration_stats.TDistributionPrecision = precision.cast<double>(); iteration_stats.PriorLogLikelihood = cfg.Mu * initial().log().squaredNorm(); total_error = -ll;//iteration_stats.TDistributionLogLikelihood + iteration_stats.PriorLogLikelihood; itctx_.LastError = itctx_.Error; itctx_.Error = total_error; // sw_error[itctx_.Level].stopAndPrint(); // accept the last increment? accept = itctx_.Error < itctx_.LastError; if(!accept) { initial.revert(); estimate.revert(); level_stats.TerminationCriterion = TerminationCriteria::LogLikelihoodDecreased; break; } // now build equation system // sw_linsys[itctx_.Level].start(); WeightVectorType::iterator w_it = weights.begin(); Matrix2x6 J, Jw; Eigen::Vector2f Ji; Vector6 Jz; ls.initialize(1); for(PointIterator e_it = compute_residuals_result.first_point_error; e_it != compute_residuals_result.last_point_error; ++e_it, ++w_it) { computeJacobianOfProjectionAndTransformation(e_it->getPointVec4f(), Jw); compute3rdRowOfJacobianOfTransformation(e_it->getPointVec4f(), Jz); J.row(0) = e_it->getIntensityDerivativeVec2f().transpose() * Jw; J.row(1) = e_it->getDepthDerivativeVec2f().transpose() * Jw - Jz.transpose(); ls.update(J, e_it->getIntensityAndDepthVec2f(), (*w_it) * precision); } ls.finish(); A = ls.A.cast<double>() + cfg.Mu * Matrix6d::Identity(); b = ls.b.cast<double>() + cfg.Mu * initial().log(); x = A.ldlt().solve(b); // sw_linsys[itctx_.Level].stopAndPrint(); iteration_stats.EstimateIncrement = x; iteration_stats.EstimateInformation = A; itctx_.Iteration++; // sw_it[itctx_.Level].stopAndPrint(); } while(accept && x.lpNorm<Eigen::Infinity>() > cfg.Precision && !itctx_.IterationsExceeded()); if(x.lpNorm<Eigen::Infinity>() <= cfg.Precision) level_stats.TerminationCriterion = TerminationCriteria::IncrementTooSmall; if(itctx_.IterationsExceeded()) level_stats.TerminationCriterion = TerminationCriteria::IterationsExceeded; // sw_level[itctx_.Level].stopAndPrint(); } LevelStats& last_level = result.Statistics.Levels.back(); IterationStats& last_iteration = last_level.TerminationCriterion != TerminationCriteria::LogLikelihoodDecreased ? last_level.Iterations[last_level.Iterations.size() - 1] : last_level.Iterations[last_level.Iterations.size() - 2]; result.Transformation = estimate().inverse().matrix(); result.Information = last_iteration.EstimateInformation * 0.008 * 0.008; result.LogLikelihood = last_iteration.TDistributionLogLikelihood + last_iteration.PriorLogLikelihood; return success; }