예제 #1
0
template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::downsample (const FloatImageConstPtr& input,
                                                                      FloatImageConstPtr& output,
                                                                      FloatImageConstPtr& output_grad_x,
                                                                      FloatImageConstPtr& output_grad_y) const
{
  downsample (input, output);
  FloatImagePtr grad_x (new FloatImage (input->width, input->height));
  FloatImagePtr grad_y (new FloatImage (input->width, input->height));
  derivatives (*output, *grad_x, *grad_y);
  output_grad_x = grad_x;
  output_grad_y = grad_y;
}
  void ElasticCableRayleighDamping<StressStrainLaw>::damping_residual( bool compute_jacobian,
                                                                       AssemblyContext& context,
                                                                       CachedValues& /*cache*/)
  {
    // First, do the "mass" contribution
    this->mass_residual_impl(compute_jacobian,
                               context,
                               &libMesh::FEMContext::interior_rate,
                               &libMesh::DiffContext::get_elem_solution_rate_derivative,
                               _mu_factor);

    // Now do the stiffness contribution
    const unsigned int n_u_dofs = context.get_dof_indices(this->_disp_vars.u()).size();

    const std::vector<libMesh::Real> &JxW =
      this->get_fe(context)->get_JxW();

    // Residuals that we're populating
    libMesh::DenseSubVector<libMesh::Number> &Fu = context.get_elem_residual(this->_disp_vars.u());
    libMesh::DenseSubVector<libMesh::Number> &Fv = context.get_elem_residual(this->_disp_vars.v());
    libMesh::DenseSubVector<libMesh::Number> &Fw = context.get_elem_residual(this->_disp_vars.w());

    //Grab the Jacobian matrix as submatrices
    //libMesh::DenseMatrix<libMesh::Number> &K = context.get_elem_jacobian();
    libMesh::DenseSubMatrix<libMesh::Number> &Kuu = context.get_elem_jacobian(this->_disp_vars.u(),this->_disp_vars.u());
    libMesh::DenseSubMatrix<libMesh::Number> &Kuv = context.get_elem_jacobian(this->_disp_vars.u(),this->_disp_vars.v());
    libMesh::DenseSubMatrix<libMesh::Number> &Kuw = context.get_elem_jacobian(this->_disp_vars.u(),this->_disp_vars.w());
    libMesh::DenseSubMatrix<libMesh::Number> &Kvu = context.get_elem_jacobian(this->_disp_vars.v(),this->_disp_vars.u());
    libMesh::DenseSubMatrix<libMesh::Number> &Kvv = context.get_elem_jacobian(this->_disp_vars.v(),this->_disp_vars.v());
    libMesh::DenseSubMatrix<libMesh::Number> &Kvw = context.get_elem_jacobian(this->_disp_vars.v(),this->_disp_vars.w());
    libMesh::DenseSubMatrix<libMesh::Number> &Kwu = context.get_elem_jacobian(this->_disp_vars.w(),this->_disp_vars.u());
    libMesh::DenseSubMatrix<libMesh::Number> &Kwv = context.get_elem_jacobian(this->_disp_vars.w(),this->_disp_vars.v());
    libMesh::DenseSubMatrix<libMesh::Number> &Kww = context.get_elem_jacobian(this->_disp_vars.w(),this->_disp_vars.w());

    unsigned int n_qpoints = context.get_element_qrule().n_points();

    // All shape function gradients are w.r.t. master element coordinates
    const std::vector<std::vector<libMesh::Real> >& dphi_dxi = this->get_fe(context)->get_dphidxi();

    const libMesh::DenseSubVector<libMesh::Number>& u_coeffs = context.get_elem_solution( this->_disp_vars.u() );
    const libMesh::DenseSubVector<libMesh::Number>& v_coeffs = context.get_elem_solution( this->_disp_vars.v() );
    const libMesh::DenseSubVector<libMesh::Number>& w_coeffs = context.get_elem_solution( this->_disp_vars.w() );

    const libMesh::DenseSubVector<libMesh::Number>& dudt_coeffs = context.get_elem_solution_rate( this->_disp_vars.u() );
    const libMesh::DenseSubVector<libMesh::Number>& dvdt_coeffs = context.get_elem_solution_rate( this->_disp_vars.v() );
    const libMesh::DenseSubVector<libMesh::Number>& dwdt_coeffs = context.get_elem_solution_rate( this->_disp_vars.w() );

    // Need these to build up the covariant and contravariant metric tensors
    const std::vector<libMesh::RealGradient>& dxdxi  = this->get_fe(context)->get_dxyzdxi();

    const unsigned int dim = 1; // The cable dimension is always 1 for this physics

    for (unsigned int qp=0; qp != n_qpoints; qp++)
      {
        // Gradients are w.r.t. master element coordinates
        libMesh::Gradient grad_u, grad_v, grad_w;
        libMesh::Gradient dgradu_dt, dgradv_dt, dgradw_dt;

        for( unsigned int d = 0; d < n_u_dofs; d++ )
          {
            libMesh::RealGradient u_gradphi( dphi_dxi[d][qp] );
            grad_u += u_coeffs(d)*u_gradphi;
            grad_v += v_coeffs(d)*u_gradphi;
            grad_w += w_coeffs(d)*u_gradphi;

            dgradu_dt += dudt_coeffs(d)*u_gradphi;
            dgradv_dt += dvdt_coeffs(d)*u_gradphi;
            dgradw_dt += dwdt_coeffs(d)*u_gradphi;
          }

        libMesh::RealGradient grad_x( dxdxi[qp](0) );
        libMesh::RealGradient grad_y( dxdxi[qp](1) );
        libMesh::RealGradient grad_z( dxdxi[qp](2) );

        libMesh::TensorValue<libMesh::Real> a_cov, a_contra, A_cov, A_contra;
        libMesh::Real lambda_sq = 0;

        this->compute_metric_tensors( qp, *(this->get_fe(context)), context,
                                      grad_u, grad_v, grad_w,
                                      a_cov, a_contra, A_cov, A_contra,
                                      lambda_sq );

        // Compute stress tensor
        libMesh::TensorValue<libMesh::Real> tau;
        ElasticityTensor C;
        this->_stress_strain_law.compute_stress_and_elasticity(dim,a_contra,a_cov,A_contra,A_cov,tau,C);

        libMesh::Real jac = JxW[qp];

        for (unsigned int i=0; i != n_u_dofs; i++)
          {
            libMesh::RealGradient u_gradphi( dphi_dxi[i][qp] );

            libMesh::Real u_diag_factor = _lambda_factor*this->_A*jac*tau(0,0)*dgradu_dt(0)*u_gradphi(0);
            libMesh::Real v_diag_factor = _lambda_factor*this->_A*jac*tau(0,0)*dgradv_dt(0)*u_gradphi(0);
            libMesh::Real w_diag_factor = _lambda_factor*this->_A*jac*tau(0,0)*dgradw_dt(0)*u_gradphi(0);

            const libMesh::Real C1 = _lambda_factor*this->_A*jac*C(0,0,0,0)*u_gradphi(0);

            const libMesh::Real gamma_u = (grad_x(0)+grad_u(0));
            const libMesh::Real gamma_v = (grad_y(0)+grad_v(0));
            const libMesh::Real gamma_w = (grad_z(0)+grad_w(0));

            const libMesh::Real x_term = C1*gamma_u;
            const libMesh::Real y_term = C1*gamma_v;
            const libMesh::Real z_term = C1*gamma_w;

            const libMesh::Real dt_term = dgradu_dt(0)*gamma_u + dgradv_dt(0)*gamma_v + dgradw_dt(0)*gamma_w;

            Fu(i) += u_diag_factor + x_term*dt_term;
            Fv(i) += v_diag_factor + y_term*dt_term;
            Fw(i) += w_diag_factor + z_term*dt_term;
          }

        if( compute_jacobian )
          {
            for(unsigned int i=0; i != n_u_dofs; i++)
              {
                libMesh::RealGradient u_gradphi_I( dphi_dxi[i][qp] );

                for(unsigned int j=0; j != n_u_dofs; j++)
                  {
                    libMesh::RealGradient u_gradphi_J( dphi_dxi[j][qp] );

                    libMesh::Real common_factor = _lambda_factor*this->_A*jac*u_gradphi_I(0);

                    const libMesh::Real diag_term_1 = common_factor*tau(0,0)*u_gradphi_J(0)*context.get_elem_solution_rate_derivative();

                    const libMesh::Real dgamma_du = ( u_gradphi_J(0)*(grad_x(0)+grad_u(0)) );

                    const libMesh::Real dgamma_dv = ( u_gradphi_J(0)*(grad_y(0)+grad_v(0)) );

                    const libMesh::Real dgamma_dw = ( u_gradphi_J(0)*(grad_z(0)+grad_w(0)) );

                    const libMesh::Real diag_term_2_factor = common_factor*C(0,0,0,0)*context.get_elem_solution_derivative();

                    Kuu(i,j) += diag_term_1 + dgradu_dt(0)*diag_term_2_factor*dgamma_du;
                    Kuv(i,j) += dgradu_dt(0)*diag_term_2_factor*dgamma_dv;
                    Kuw(i,j) += dgradu_dt(0)*diag_term_2_factor*dgamma_dw;

                    Kvu(i,j) += dgradv_dt(0)*diag_term_2_factor*dgamma_du;
                    Kvv(i,j) += diag_term_1 + dgradv_dt(0)*diag_term_2_factor*dgamma_dv;
                    Kvw(i,j) += dgradv_dt(0)*diag_term_2_factor*dgamma_dw;

                    Kwu(i,j) += dgradw_dt(0)*diag_term_2_factor*dgamma_du;
                    Kwv(i,j) += dgradw_dt(0)*diag_term_2_factor*dgamma_dv;
                    Kww(i,j) += diag_term_1 + dgradw_dt(0)*diag_term_2_factor*dgamma_dw;

                    const libMesh::Real C1 = common_factor*C(0,0,0,0);

                    const libMesh::Real gamma_u = (grad_x(0)+grad_u(0));
                    const libMesh::Real gamma_v = (grad_y(0)+grad_v(0));
                    const libMesh::Real gamma_w = (grad_z(0)+grad_w(0));

                    const libMesh::Real x_term = C1*gamma_u;
                    const libMesh::Real y_term = C1*gamma_v;
                    const libMesh::Real z_term = C1*gamma_w;

                    const libMesh::Real ddtterm_du = u_gradphi_J(0)*(gamma_u*context.get_elem_solution_rate_derivative()
                                                                     + dgradu_dt(0)*context.get_elem_solution_derivative());

                    const libMesh::Real ddtterm_dv = u_gradphi_J(0)*(gamma_v*context.get_elem_solution_rate_derivative()
                                                                     + dgradv_dt(0)*context.get_elem_solution_derivative());

                    const libMesh::Real ddtterm_dw = u_gradphi_J(0)*(gamma_w*context.get_elem_solution_rate_derivative()
                                                                     + dgradw_dt(0)*context.get_elem_solution_derivative());

                    Kuu(i,j) += x_term*ddtterm_du;
                    Kuv(i,j) += x_term*ddtterm_dv;
                    Kuw(i,j) += x_term*ddtterm_dw;

                    Kvu(i,j) += y_term*ddtterm_du;
                    Kvv(i,j) += y_term*ddtterm_dv;
                    Kvw(i,j) += y_term*ddtterm_dw;

                    Kwu(i,j) += z_term*ddtterm_du;
                    Kwv(i,j) += z_term*ddtterm_dv;
                    Kww(i,j) += z_term*ddtterm_dw;

                    const libMesh::Real dt_term = dgradu_dt(0)*gamma_u + dgradv_dt(0)*gamma_v + dgradw_dt(0)*gamma_w;

                    // Here, we're missing derivatives of C(0,0,0,0) w.r.t. strain
                    // Nonzero for hyperelasticity models
                    const libMesh::Real dxterm_du = C1*u_gradphi_J(0)*context.get_elem_solution_derivative();
                    const libMesh::Real dyterm_dv = dxterm_du;
                    const libMesh::Real dzterm_dw = dxterm_du;

                    Kuu(i,j) += dxterm_du*dt_term;
                    Kvv(i,j) += dyterm_dv*dt_term;
                    Kww(i,j) += dzterm_dw*dt_term;

                  } // end j-loop
              } // end i-loop
          } // end if(compute_jacobian)
      } // end qp loop
  }
예제 #3
0
template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::computePyramids (const PointCloudInConstPtr& input,
                                                                    std::vector<FloatImageConstPtr>& pyramid,
                                                                    pcl::InterpolationType border_type) const
{
  int step = 3;
  pyramid.resize (step * nb_levels_);

  FloatImageConstPtr previous;
  FloatImagePtr tmp (new FloatImage (input->width, input->height));
#ifdef _OPENMP
#pragma omp parallel for num_threads (threads_)
#endif
  for (int i = 0; i < static_cast<int> (input->size ()); ++i)
    tmp->points[i] = intensity_ (input->points[i]);
  previous = tmp;

  FloatImagePtr img (new FloatImage (previous->width + 2*track_width_,
                                     previous->height + 2*track_height_));

  pcl::copyPointCloud (*tmp, *img, track_height_, track_height_, track_width_, track_width_,
                       border_type, 0.f);
  pyramid[0] = img;

  // compute first level gradients
  FloatImagePtr g_x (new FloatImage (input->width, input->height));
  FloatImagePtr g_y (new FloatImage (input->width, input->height));
  derivatives (*img, *g_x, *g_y);
  // copy to bigger clouds
  FloatImagePtr grad_x (new FloatImage (previous->width + 2*track_width_,
                                        previous->height + 2*track_height_));
  pcl::copyPointCloud (*g_x, *grad_x, track_height_, track_height_, track_width_, track_width_,
                       pcl::BORDER_CONSTANT, 0.f);
  pyramid[1] = grad_x;

  FloatImagePtr grad_y (new FloatImage (previous->width + 2*track_width_,
                                        previous->height + 2*track_height_));
  pcl::copyPointCloud (*g_y, *grad_y, track_height_, track_height_, track_width_, track_width_,
                       pcl::BORDER_CONSTANT, 0.f);
  pyramid[2] = grad_y;

  for (int level = 1; level < nb_levels_; ++level)
  {
    // compute current level and current level gradients
    FloatImageConstPtr current;
    FloatImageConstPtr g_x;
    FloatImageConstPtr g_y;
    downsample (previous, current, g_x, g_y);
    // copy to bigger clouds
    FloatImagePtr image (new FloatImage (current->width + 2*track_width_,
                                         current->height + 2*track_height_));
    pcl::copyPointCloud (*current, *image, track_height_, track_height_, track_width_, track_width_,
                         border_type, 0.f);
    pyramid[level*step] = image;
    FloatImagePtr gradx (new FloatImage (g_x->width + 2*track_width_, g_x->height + 2*track_height_));
    pcl::copyPointCloud (*g_x, *gradx, track_height_, track_height_, track_width_, track_width_,
                         pcl::BORDER_CONSTANT, 0.f);
    pyramid[level*step + 1] = gradx;
    FloatImagePtr grady (new FloatImage (g_y->width + 2*track_width_, g_y->height + 2*track_height_));
    pcl::copyPointCloud (*g_y, *grady, track_height_, track_height_, track_width_, track_width_,
                         pcl::BORDER_CONSTANT, 0.f);
    pyramid[level*step + 2] = grady;
    // set the new level
    previous = current;
  }
}