Ejemplo n.º 1
0
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;
}
Ejemplo n.º 2
0
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;
}
Ejemplo n.º 3
0
  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);
  }