obvious::Matrix ThreadLocalize::doRegistration(obvious::SensorPolar2D* sensor, obvious::Matrix* M, obvious::Matrix* Mvalid, obvious::Matrix* N, obvious::Matrix* Nvalid, obvious::Matrix* S, obvious::Matrix* Svalid) { obvious::Matrix T44(4,4); T44.setIdentity(); obvious::Matrix T(3,3); //wofür ist das? #ifdef TRACE obvious::Matrix T_old(3,3); #endif //RANSAC pre-registration (rough) switch(_regMode) { case ICP: //no pre-registration break; case EXP: //todo: check normals N for matching function (Daniel Ammon, Tobias Fink) T = _RandomNormalMatcher->match(M, _maskM, NULL, S, _maskS, obvious::deg2rad(_ranPhiMax), _trnsMax, sensor->getAngularResolution()); T44(0, 0) = T(0, 0); T44(0, 1) = T(0, 1); T44(0, 3) = T(0, 2); T44(1, 0) = T(1, 0); T44(1, 1) = T(1, 1); T44(1, 3) = T(1, 2); break; case PDF: //todo: check normals N for matching function (Daniel Ammon, Tobias Fink) T = _PDFMatcher->match(M, _maskM, NULL, S, _maskS, obvious::deg2rad(_ranPhiMax), _trnsMax, sensor->getAngularResolution()); T44(0, 0) = T(0, 0); T44(0, 1) = T(0, 1); T44(0, 3) = T(0, 2); T44(1, 0) = T(1, 0); T44(1, 1) = T(1, 1); T44(1, 3) = T(1, 2); break; case TSD: //todo: check normals N for matching function (Daniel Ammon, Tobias Fink T = _TSD_PDFMatcher->match(sensor->getTransformation(), M, _maskM, NULL, S, _maskS, obvious::deg2rad(_ranPhiMax), _trnsMax, sensor->getAngularResolution()); T44(0, 0) = T(0, 0); T44(0, 1) = T(0, 1); T44(0, 3) = T(0, 2); T44(1, 0) = T(1, 0); T44(1, 1) = T(1, 1); T44(1, 3) = T(1, 2); break; default: //no pre-registration break; } _icp->reset(); obvious::Matrix P = sensor->getTransformation(); _filterBounds->setPose(&P); _icp->setModel(Mvalid, Nvalid); _icp->setScene(Svalid); double rms = 0.0; unsigned int pairs = 0; unsigned int it = 0; _icp->iterate(&rms, &pairs, &it, &T44); T = _icp->getFinalTransformation(); // std::cout << __PRETTY_FUNCTION__ << " _useOdomRescue = " << _useOdomRescue << std::endl; // std::cout << __PRETTY_FUNCTION__ << " _odomTfIsValid = " << _odomTfIsValid << std::endl; // std::cout << "slam Transformation before odomRescueCheck: T_slam = \n" << T << std::endl; //if(_useOdomRescue && _odomTfIsValid) // _odomAnalyzer->odomRescueCheck(T); //_odomAnalyzer->odomRescueCheck(T); return T; }
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; }
void PhaseResidual<EvalT, Traits>:: evaluateFields(typename Traits::EvalData workset) { // time step ScalarT dt = deltaTime(0); typedef Intrepid2::FunctionSpaceTools<PHX::Device> FST; if (dt == 0.0) dt = 1.0e-15; //grab old temperature Albany::MDArray T_old = (*workset.stateArrayPtr)[Temperature_Name_]; // Compute Temp rate for (std::size_t cell = 0; cell < workset.numCells; ++cell) { for (std::size_t qp = 0; qp < num_qps_; ++qp) { T_dot_(cell, qp) = (T_(cell, qp) - T_old(cell, qp)) / dt; } } // diffusive term FST::scalarMultiplyDataData<ScalarT> (term1_, k_.get_view(), T_grad_.get_view()); // FST::integrate(residual_, term1_, w_grad_bf_, false); //Using for loop to calculate the residual // zero out residual for (int cell = 0; cell < workset.numCells; ++cell) { for (int node = 0; node < num_nodes_; ++node) { residual_(cell,node) = 0.0; } } // for (int cell = 0; cell < workset.numCells; ++cell) { // for (int qp = 0; qp < num_qps_; ++qp) { // for (int node = 0; node < num_nodes_; ++node) { // for (int i = 0; i < num_dims_; ++i) { // residual_(cell,node) += w_grad_bf_(cell,node,qp,i) * term1_(cell,qp,i); // } // } // } // } //THESE ARE HARD CODED NOW. NEEDS TO BE CHANGED TO USER INPUT LATER ScalarT Coeff_volExp = 65.2e-6; //per kelvins ScalarT Ini_temp = 300; //kelvins if (hasConsolidation_) { for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { //Use if consolidation and expansion is considered // porosity_function1 = pow( ((1.0 - porosity_(cell, qp)) / ((1+Coeff_volExp*(T_(cell,qp) -Ini_temp))*(1.0 - Initial_porosity))), 2); // porosity_function2 = (1+Coeff_volExp*(T_(cell,qp) -Ini_temp))*(1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); //Use if only consolidation is considered porosity_function1 = pow( ((1.0 - porosity_(cell, qp)) / (1.0 - Initial_porosity)), 2); porosity_function2 = (1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); //In the model that is currently used, the Z-axis corresponds to the depth direction. Hence the term porosity //function1 is multiplied with the second term. residual_(cell, node) += porosity_function2 * ( w_grad_bf_(cell, node, qp, 0) * term1_(cell, qp, 0) + w_grad_bf_(cell, node, qp, 1) * term1_(cell, qp, 1) + porosity_function1 * w_grad_bf_(cell, node, qp, 2) * term1_(cell, qp, 2)); } } } // heat source from laser for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { //Use if consolidation and expansion is considered // porosity_function2 = (1+Coeff_volExp*(T_(cell,qp) -Ini_temp))*(1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); //Use if only consolidation is considered porosity_function2 = (1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); residual_(cell, node) -= porosity_function2 * (w_bf_(cell, node, qp) * laser_source_(cell, qp)); } } } // all other problem sources for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { //Use if consolidation and expansion is considered // porosity_function2 = (1+Coeff_volExp*(T_(cell,qp) -Ini_temp))*(1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); //Use if only consolidation is considered porosity_function2 = (1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); residual_(cell, node) -= porosity_function2 * (w_bf_(cell, node, qp) * source_(cell, qp)); } } } // transient term for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { //Use if consolidation and expansion is considered // porosity_function2 = (1+Coeff_volExp*(T_(cell,qp) -Ini_temp))*(1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); //Use if only consolidation is considered porosity_function2 = (1.0 - Initial_porosity) / (1.0 - porosity_(cell, qp)); residual_(cell, node) += porosity_function2 * (w_bf_(cell, node, qp) * energyDot_(cell, qp)); } } } } else { // does not have consolidation for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { residual_(cell, node) += ( w_grad_bf_(cell, node, qp, 0) * term1_(cell, qp, 0) + w_grad_bf_(cell, node, qp, 1) * term1_(cell, qp, 1) + w_grad_bf_(cell, node, qp, 2) * term1_(cell, qp, 2)); } } } // heat source from laser for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { residual_(cell, node) -= (w_bf_(cell, node, qp) * laser_source_(cell, qp)); } } } // all other problem sources for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { residual_(cell, node) -= (w_bf_(cell, node, qp) * source_(cell, qp)); } } } // transient term for (int cell = 0; cell < workset.numCells; ++cell) { for (int qp = 0; qp < num_qps_; ++qp) { for (int node = 0; node < num_nodes_; ++node) { residual_(cell, node) += (w_bf_(cell, node, qp) * energyDot_(cell, qp)); } } } } // heat source from laser //PHAL::scale(laser_source_, -1.0); //FST::integrate(residual_, laser_source_, w_bf_, true); // all other problem sources //PHAL::scale(source_, -1.0); //FST::integrate(residual_, source_, w_bf_, true); // transient term //FST::integrate(residual_, energyDot_, w_bf_, true); }