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);
}
