C++ Matrix::coeffRef方法代码示例

/*
 *      You can use GPU for that
 * */
void
Transform_Cloud (    matrix <Point_XYZI> & In_Cloud,
                     matrix <Point_XYZI> & Out_Cloud,
                     Eigen::Affine3f & Transform )
{
    if ( & In_Cloud != & Out_Cloud )
        Out_Cloud.Set_Dimensions ( In_Cloud.Get_Dimensions() );

    // Dataset might contain NaNs and Infs, so check for them first,
    // otherwise we get errors during the multiplication (?)
    for (size_t i = 0; i < In_Cloud.Get_Num_Of_Elem (); ++i) {

        XYZI cur_point = In_Cloud.data[i];

        if (    ! isfinite (cur_point.x) || 
                ! isfinite (cur_point.y) || 
                ! isfinite (cur_point.z) )
            continue;

        // FIXME - rewrite - 
        // too complex 
        // use SSE ???
        Eigen::Matrix<float, 3, 1> pt ( cur_point.x, cur_point.y, cur_point.z );
        XYZI result;
        result.x = static_cast<float> (Transform (0, 0) * pt.coeffRef (0) + Transform (0, 1) * pt.coeffRef (1) + Transform (0, 2) * pt.coeffRef (2) + Transform (0, 3));
        result.y = static_cast<float> (Transform (1, 0) * pt.coeffRef (0) + Transform (1, 1) * pt.coeffRef (1) + Transform (1, 2) * pt.coeffRef (2) + Transform (1, 3));
        result.z = static_cast<float> (Transform (2, 0) * pt.coeffRef (0) + Transform (2, 1) * pt.coeffRef (1) + Transform (2, 2) * pt.coeffRef (2) + Transform (2, 3));
        result.w = cur_point.w;
        Out_Cloud.data[i] = result;
    }
}

template <typename PointT, typename Scalar> void
pcl::transformPointCloud (const pcl::PointCloud<PointT> &cloud_in, 
                          const std::vector<int> &indices, 
                          pcl::PointCloud<PointT> &cloud_out,
                          const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform,
                          bool copy_all_fields)
{
  size_t npts = indices.size ();
  // In order to transform the data, we need to remove NaNs
  cloud_out.is_dense = cloud_in.is_dense;
  cloud_out.header   = cloud_in.header;
  cloud_out.width    = static_cast<int> (npts);
  cloud_out.height   = 1;
  cloud_out.points.resize (npts);
  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
  cloud_out.sensor_origin_      = cloud_in.sensor_origin_;

  if (cloud_in.is_dense)
  {
    // If the dataset is dense, simply transform it!
    for (size_t i = 0; i < npts; ++i)
    {
      // Copy fields first, then transform xyz data
      if (copy_all_fields)
        cloud_out.points[i] = cloud_in.points[indices[i]];
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
  else
  {
    // Dataset might contain NaNs and Infs, so check for them first,
    // otherwise we get errors during the multiplication (?)
    for (size_t i = 0; i < npts; ++i)
    {
      if (copy_all_fields)
        cloud_out.points[i] = cloud_in.points[indices[i]];
      if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
          !pcl_isfinite (cloud_in.points[indices[i]].y) || 
          !pcl_isfinite (cloud_in.points[indices[i]].z))
        continue;
      //cloud_out.points[i].getVector3fMap () = transform*cloud_out.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));
    }
  }
}

template <typename PointT, typename Scalar> inline unsigned int
pcl::computeCovarianceMatrix (const pcl::PointCloud<PointT> &cloud,
                              Eigen::Matrix<Scalar, 3, 3> &covariance_matrix)
{
  // create the buffer on the stack which is much faster than using cloud[indices[i]] and centroid as a buffer
  Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor> accu = Eigen::Matrix<Scalar, 1, 6, Eigen::RowMajor>::Zero ();

  unsigned int point_count;
  if (cloud.is_dense)
  {
    point_count = static_cast<unsigned int> (cloud.size ());
    // For each point in the cloud
    for (size_t i = 0; i < point_count; ++i)
    {
      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
    }
  }
  else
  {
    point_count = 0;
    for (size_t i = 0; i < cloud.size (); ++i)
    {
      if (!isFinite (cloud[i]))
        continue;

      accu [0] += cloud[i].x * cloud[i].x;
      accu [1] += cloud[i].x * cloud[i].y;
      accu [2] += cloud[i].x * cloud[i].z;
      accu [3] += cloud[i].y * cloud[i].y;
      accu [4] += cloud[i].y * cloud[i].z;
      accu [5] += cloud[i].z * cloud[i].z;
      ++point_count;
    }
  }

  if (point_count != 0)
  {
    accu /= static_cast<Scalar> (point_count);
    covariance_matrix.coeffRef (0) = accu [0];
    covariance_matrix.coeffRef (1) = covariance_matrix.coeffRef (3) = accu [1];
    covariance_matrix.coeffRef (2) = covariance_matrix.coeffRef (6) = accu [2];
    covariance_matrix.coeffRef (4) = accu [3];
    covariance_matrix.coeffRef (5) = covariance_matrix.coeffRef (7) = accu [4];
    covariance_matrix.coeffRef (8) = accu [5];
  }
  return (point_count);
}

template <typename PointT, typename Scalar> void
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                     const std::vector<int> &indices, 
                                     pcl::PointCloud<PointT> &cloud_out,
                                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  size_t npts = indices.size ();
  // In order to transform the data, we need to remove NaNs
  cloud_out.is_dense = cloud_in.is_dense;
  cloud_out.header   = cloud_in.header;
  cloud_out.width    = static_cast<int> (npts);
  cloud_out.height   = 1;
  cloud_out.points.resize (npts);
  cloud_out.sensor_orientation_ = cloud_in.sensor_orientation_;
  cloud_out.sensor_origin_      = cloud_in.sensor_origin_;

  // If the data is dense, we don't need to check for NaN
  if (cloud_in.is_dense)
  {
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
    {
      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));

      // Rotate normals
      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
    }
  }
  // Dataset might contain NaNs and Infs, so check for them first.
  else
  {
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
    {
      if (!pcl_isfinite (cloud_in.points[indices[i]].x) || 
          !pcl_isfinite (cloud_in.points[indices[i]].y) || 
          !pcl_isfinite (cloud_in.points[indices[i]].z))
        continue;

      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[indices[i]].x, cloud_in[indices[i]].y, cloud_in[indices[i]].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));

      // Rotate normals
      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[indices[i]].normal_x, cloud_in[indices[i]].normal_y, cloud_in[indices[i]].normal_z);
      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
    }
  }
}

template <typename PointT, typename Scalar> void
pcl::transformPointCloudWithNormals (const pcl::PointCloud<PointT> &cloud_in, 
                                     pcl::PointCloud<PointT> &cloud_out,
                                     const Eigen::Transform<Scalar, 3, Eigen::Affine> &transform)
{
  if (&cloud_in != &cloud_out)
  {
    // Note: could be replaced by cloud_out = cloud_in
    cloud_out.header   = cloud_in.header;
    cloud_out.width    = cloud_in.width;
    cloud_out.height   = cloud_in.height;
    cloud_out.is_dense = cloud_in.is_dense;
    cloud_out.points.reserve (cloud_out.points.size ());
    cloud_out.points.assign (cloud_in.points.begin (), cloud_in.points.end ());
  }

  // If the data is dense, we don't need to check for NaN
  if (cloud_in.is_dense)
  {
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
    {
      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));

      // Rotate normals (WARNING: transform.rotation () uses SVD internally!)
      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
    }
  }
  // Dataset might contain NaNs and Infs, so check for them first.
  else
  {
    for (size_t i = 0; i < cloud_out.points.size (); ++i)
    {
      if (!pcl_isfinite (cloud_in.points[i].x) || 
          !pcl_isfinite (cloud_in.points[i].y) || 
          !pcl_isfinite (cloud_in.points[i].z))
        continue;

      //cloud_out.points[i].getVector3fMap() = transform * cloud_in.points[i].getVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> pt (cloud_in[i].x, cloud_in[i].y, cloud_in[i].z);
      cloud_out[i].x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
      cloud_out[i].y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
      cloud_out[i].z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));

      // Rotate normals
      //cloud_out.points[i].getNormalVector3fMap() = transform.rotation () * cloud_in.points[i].getNormalVector3fMap ();
      Eigen::Matrix<Scalar, 3, 1> nt (cloud_in[i].normal_x, cloud_in[i].normal_y, cloud_in[i].normal_z);
      cloud_out[i].normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
      cloud_out[i].normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
      cloud_out[i].normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
    }
  }
}

void GreenStrain_LIMSolver2D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess)
{
  // green strain tensor energy
  Eigen::Matrix<double,2,3> S;
  for(int t=0;t<mesh->Triangles->rows();t++)
  {
    Eigen::Vector2d A(x[TriangleVertexIdx.coeff(0,t)],x[TriangleVertexIdx.coeff(1,t)]);
    Eigen::Vector2d B(x[TriangleVertexIdx.coeff(2,t)],x[TriangleVertexIdx.coeff(3,t)]);
    Eigen::Vector2d C(x[TriangleVertexIdx.coeff(4,t)],x[TriangleVertexIdx.coeff(5,t)]);

    Eigen::Matrix<double,2,3> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;

    // hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x
    Eigen::Matrix3d VTV = V.transpose()*V;
    Eigen::Matrix3d MMT = MMTs.block<3,3>(0,3*t);
    Eigen::Matrix<double,2,3> VMMT = V*MMT;
    Eigen::Matrix3d MMTVTV = MMT*VTV;

    int numElem = 0;
    for(int r=0;r<6;r++)
    {
      S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(2,3);
      S.coeffRef(r) = 1;

      Eigen::Matrix<double,2,3> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT);
      
      for(int c=r;c<6;c++)
        *denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider;
    }
  }
}

 /* ctor for cholesky function
  *
  * Stores varis for A
  * Instantiates and stores varis for L
  * Instantiates and stores dummy vari for
  * upper triangular part of var result returned
  * in cholesky_decompose function call
  *
  * variRefL aren't on the chainable
  * autodiff stack, only used for storage
  * and computation. Note that varis for
  * L are constructed externally in
  * cholesky_decompose.
  *
  * @param matrix A
  * @param matrix L, cholesky factor of A
  * */
 cholesky_decompose_v_vari(const Eigen::Matrix<var, -1, -1>& A,
                           const Eigen::Matrix<double, -1, -1>& L_A)
   : vari(0.0),
     M_(A.rows()),
     variRefA_(ChainableStack::memalloc_.alloc_array<vari*>
               (A.rows() * (A.rows() + 1) / 2)),
     variRefL_(ChainableStack::memalloc_.alloc_array<vari*>
               (A.rows() * (A.rows() + 1) / 2)) {
   size_t accum = 0;
   size_t accum_i = accum;
   for (size_type j = 0; j < M_; ++j) {
     for (size_type i = j; i < M_; ++i) {
       accum_i += i;
       size_t pos = j + accum_i;
       variRefA_[pos] = A.coeffRef(i, j).vi_;
       variRefL_[pos] = new vari(L_A.coeffRef(i, j), false);
     }
     accum += j;
     accum_i = accum;
   }
 }

ValueType compare(const Eigen::Matrix<ValueType, _Rows, _Cols, _Option> &mat,
                  const ValueType value, const ValueType eps = 1e-12) {
  ValueType v;
  int num_eq = 0;
  if (_Option == Eigen::RowMajor) {
    for (int i = 0; i < mat.rows(); ++i) {
      for (int j = 0; j < mat.cols(); ++j) {
        v = mat.coeffRef(i, j);
        if (compare(v, value, eps))
          ++num_eq;
      }
    }
  } else {
    for (int j = 0; j < mat.cols(); ++j) {
      for (int i = 0; i < mat.rows(); ++i) {
        v = mat.coeffRef(i, j);
        if (compare(v, value, eps))
          ++num_eq;
      }
    }
  }
  return num_eq;
}

    inline
    Eigen::Matrix<typename boost::math::tools::promote_args<T1, T2>::type,
                  Eigen::Dynamic, 1>
    csr_matrix_times_vector(const int& m,
                            const int& n,
                            const Eigen::Matrix<T1, Eigen::Dynamic, 1>& w,
                            const std::vector<int>& v,
                            const std::vector<int>& u,
                            const Eigen::Matrix<T2, Eigen::Dynamic, 1>& b) {
      typedef typename boost::math::tools::promote_args<T1, T2>::type
        result_t;

      check_positive("csr_matrix_times_vector", "m", m);
      check_positive("csr_matrix_times_vector", "n", n);
      check_size_match("csr_matrix_times_vector", "n", n, "b", b.size());
      check_size_match("csr_matrix_times_vector", "m", m, "u", u.size() - 1);
      check_size_match("csr_matrix_times_vector", "w", w.size(), "v", v.size());
      check_size_match("csr_matrix_times_vector", "u/z",
                       u[m - 1] + csr_u_to_z(u, m - 1) - 1, "v", v.size());
      for (unsigned int i = 0; i < v.size(); ++i)
        check_range("csr_matrix_times_vector", "v[]", n, v[i]);

      Eigen::Matrix<result_t, Eigen::Dynamic, 1>  result(m);
      result.setZero();
      for (int row = 0; row < m; ++row) {
        int idx = csr_u_to_z(u, row);
        int row_end_in_w = (u[row] - stan::error_index::value) + idx;
        int i = 0;  // index into dot-product segment entries.
        Eigen::Matrix<result_t, Eigen::Dynamic, 1> b_sub(idx);
        b_sub.setZero();
        for (int nze = u[row] - stan::error_index::value;
             nze < row_end_in_w; ++nze, ++i) {
          check_range("csr_matrix_times_vector", "j", n, v[nze]);
          b_sub.coeffRef(i) = b.coeffRef(v[nze] - stan::error_index::value);
        }  // loop skipped when z is zero.
        Eigen::Matrix<T1, Eigen::Dynamic, 1>
          w_sub(w.segment(u[row] - stan::error_index::value, idx));
        result.coeffRef(row) = dot_product(w_sub, b_sub);
      }
      return result;
    }

pcl::PointNormal transformPoint(
		const pcl::PointNormal & point,
		const Transform & transform)
{
	pcl::PointNormal ret;
	Eigen::Matrix<float, 3, 1> pt (point.x, point.y, point.z);
	ret.x = static_cast<float> (transform (0, 0) * pt.coeffRef (0) + transform (0, 1) * pt.coeffRef (1) + transform (0, 2) * pt.coeffRef (2) + transform (0, 3));
	ret.y = static_cast<float> (transform (1, 0) * pt.coeffRef (0) + transform (1, 1) * pt.coeffRef (1) + transform (1, 2) * pt.coeffRef (2) + transform (1, 3));
	ret.z = static_cast<float> (transform (2, 0) * pt.coeffRef (0) + transform (2, 1) * pt.coeffRef (1) + transform (2, 2) * pt.coeffRef (2) + transform (2, 3));

	// Rotate normals
	Eigen::Matrix<float, 3, 1> nt (point.normal_x, point.normal_y, point.normal_z);
	ret.normal_x = static_cast<float> (transform (0, 0) * nt.coeffRef (0) + transform (0, 1) * nt.coeffRef (1) + transform (0, 2) * nt.coeffRef (2));
	ret.normal_y = static_cast<float> (transform (1, 0) * nt.coeffRef (0) + transform (1, 1) * nt.coeffRef (1) + transform (1, 2) * nt.coeffRef (2));
	ret.normal_z = static_cast<float> (transform (2, 0) * nt.coeffRef (0) + transform (2, 1) * nt.coeffRef (1) + transform (2, 2) * nt.coeffRef (2));
	return ret;
}

void GreenStrain_LIMSolver3D::computeHessian(const Eigen::Matrix<double,Eigen::Dynamic,1>& x, const Eigen::Matrix<double*,Eigen::Dynamic,1>& hess)
{
  // green strain tensor energy
  Eigen::Matrix<double,3,4> S;
  for(int t=0;t<mesh->Tetrahedra->rows();t++)
  {
    Eigen::Vector3d A(x[TetrahedronVertexIdx.coeff(0,t)],x[TetrahedronVertexIdx.coeff(1,t)],x[TetrahedronVertexIdx.coeff(2,t)]);
    Eigen::Vector3d B(x[TetrahedronVertexIdx.coeff(3,t)],x[TetrahedronVertexIdx.coeff(4,t)],x[TetrahedronVertexIdx.coeff(5,t)]);
    Eigen::Vector3d C(x[TetrahedronVertexIdx.coeff(6,t)],x[TetrahedronVertexIdx.coeff(7,t)],x[TetrahedronVertexIdx.coeff(8,t)]);
    Eigen::Vector3d D(x[TetrahedronVertexIdx.coeff(9,t)],x[TetrahedronVertexIdx.coeff(10,t)],x[TetrahedronVertexIdx.coeff(11,t)]);

    Eigen::Matrix<double,3,4> V;
    V.col(0) = A;
    V.col(1) = B;
    V.col(2) = C;
    V.col(3) = D;

    // hessian(E) = 4*r_x'*((SMM'V'V+VMM'*(V'S+SV))*MM' - SMM')*c_x
    Eigen::Matrix<double,4,4> VTV = V.transpose()*V;
    Eigen::Matrix<double,4,4> MMT = MMTs.block<4,4>(0,4*t);
    Eigen::Matrix<double,3,4> VMMT = V*MMT;
    Eigen::Matrix<double,4,4> MMTVTV = MMT*VTV;

    int numElem = 0;
    for(int r=0;r<12;r++)
    {
      S = Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>::Zero(3,4);
      S.coeffRef(r) = 1;

      Eigen::Matrix<double,3,4> Temp = 4*((S*MMTVTV + VMMT*(V.transpose()*S+S.transpose()*V))*MMT - S*MMT);
      
      for(int c=r;c<12;c++)
        *denseHessianCoeffs(numElem++,t) += Temp.coeff(c)*Divider;
    }
  }
}

vectview<typename Base::scalar, T::DOF>
subvector(Eigen::Matrix<typename Base::scalar, Base::DOF, 1>& vec, SubManifold<T, idx> Base::*)
{
	return &vec.coeffRef(idx);
}

  void serialization_test()
  {
    std::string file("test");

    bool tbIn = true,tbOut;
    char tcIn = 't',tcOut;
    unsigned char tucIn = 'u',tucOut;
    short tsIn = 6,tsOut;
    int tiIn = -10,tiOut;
    unsigned int tuiIn = 10,tuiOut;
    float tfIn = 1.0005,tfOut;
    double tdIn = 1.000000005,tdOut;

    int* tinpIn = NULL,*tinpOut = NULL;
    float* tfpIn = new float,*tfpOut = NULL;
    *tfpIn = 1.11101;

    std::string tstrIn("test12345"),tstrOut;

    Test2 tObjIn,tObjOut;
    int ti = 2;
    tObjIn.ti = &ti;


    Test1 test1,test2,test3;
    test1.ts = "100";
    test2.ts = "200";
    test3.ts = "300";

    Test1 testA, testC;
    testA.tt = &test1;
    testA.ts = "test123";
    testA.tvt.push_back(&test2);
    testA.tvt.push_back(&test3);

    Test1 testB = testA;
    testB.ts = "400";
    testB.tvt.pop_back();

    std::pair<int,bool> tPairIn(10,true);
    std::pair<int,bool> tPairOut;

    std::vector<int> tVector1In ={1,2,3,4,5};
    std::vector<int> tVector1Out;

    std::pair<int,bool> p1(10,1);
    std::pair<int,bool> p2(1,0);
    std::pair<int,bool> p3(10000,1);
    std::vector<std::pair<int,bool> > tVector2In ={p1,p2,p3};
    std::vector<std::pair<int,bool> > tVector2Out;

    std::set<std::pair<int,bool> > tSetIn ={p1,p2,p3};
    std::set<std::pair<int,bool> > tSetOut;

    std::map<int,bool> tMapIn ={p1,p2,p3};
    std::map<int,bool> tMapOut;

    Eigen::Matrix<float,3,3> tDenseMatrixIn;
    tDenseMatrixIn << Eigen::Matrix<float,3,3>::Random();
    tDenseMatrixIn.coeffRef(0,0) = 1.00001;
    Eigen::Matrix<float,3,3> tDenseMatrixOut;

    Eigen::Matrix<float,3,3,Eigen::RowMajor> tDenseRowMatrixIn;
    tDenseRowMatrixIn << Eigen::Matrix<float,3,3,Eigen::RowMajor>::Random();
    Eigen::Matrix<float,3,3,Eigen::RowMajor> tDenseRowMatrixOut;

    Eigen::SparseMatrix<double> tSparseMatrixIn;
    tSparseMatrixIn.resize(3,3);
    tSparseMatrixIn.insert(0,0) = 1.3;
    tSparseMatrixIn.insert(1,1) = 10.2;
    tSparseMatrixIn.insert(2,2) = 100.1;
    tSparseMatrixIn.finalize();
    Eigen::SparseMatrix<double> tSparseMatrixOut;

    // binary serialization

    igl::serialize(tbIn,file);
    igl::deserialize(tbOut,file);
    assert(tbIn == tbOut);

    igl::serialize(tcIn,file);
    igl::deserialize(tcOut,file);
    assert(tcIn == tcOut);

    igl::serialize(tucIn,file);
    igl::deserialize(tucOut,file);
    assert(tucIn == tucOut);

    igl::serialize(tsIn,file);
    igl::deserialize(tsOut,file);
    assert(tsIn == tsOut);

    igl::serialize(tiIn,file);
    igl::deserialize(tiOut,file);
    assert(tiIn == tiOut);

    igl::serialize(tuiIn,file);
    igl::deserialize(tuiOut,file);
    assert(tuiIn == tuiOut);

//.........这里部分代码省略.........

template<typename PointT> void
pcl::search::OrganizedNeighbor<PointT>::estimateProjectionMatrix ()
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix_.setZero ();
  if (input_->height == 1 || input_->width == 1)
  {
    PCL_ERROR ("[pcl::%s::estimateProjectionMatrix] Input dataset is not organized!\n", getName ().c_str ());
    return;
  }

  // we just want to use every 16th column and row -> skip = 2^4
  const unsigned int skip = input_->width >> 4;
  Eigen::Matrix<Scalar, 4, 4> A = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> B = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> C = Eigen::Matrix<Scalar, 4, 4>::Zero ();
  Eigen::Matrix<Scalar, 4, 4> D = Eigen::Matrix<Scalar, 4, 4>::Zero ();

  for (unsigned yIdx = 0, idx = 0; yIdx < input_->height; yIdx += skip, idx += input_->width * (skip-1))
  {
    for (unsigned xIdx = 0; xIdx < input_->width; xIdx += skip, idx += skip)
    {
      const PointT& point = input_->points[idx];
      if (isFinite (point))
      {
        Scalar xx = point.x * point.x;
        Scalar xy = point.x * point.y;
        Scalar xz = point.x * point.z;
        Scalar yy = point.y * point.y;
        Scalar yz = point.y * point.z;
        Scalar zz = point.z * point.z;
        Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;

        A.coeffRef (0) += xx;
        A.coeffRef (1) += xy;
        A.coeffRef (2) += xz;
        A.coeffRef (3) += point.x;

        A.coeffRef (5) += yy;
        A.coeffRef (6) += yz;
        A.coeffRef (7) += point.y;

        A.coeffRef (10) += zz;
        A.coeffRef (11) += point.z;
        A.coeffRef (15) += 1.0;

        B.coeffRef (0) -= xx * xIdx;
        B.coeffRef (1) -= xy * xIdx;
        B.coeffRef (2) -= xz * xIdx;
        B.coeffRef (3) -= point.x * xIdx;

        B.coeffRef (5) -= yy * xIdx;
        B.coeffRef (6) -= yz * xIdx;
        B.coeffRef (7) -= point.y * xIdx;

        B.coeffRef (10) -= zz * xIdx;
        B.coeffRef (11) -= point.z * xIdx;

        B.coeffRef (15) -= xIdx;

        C.coeffRef (0) -= xx * yIdx;
        C.coeffRef (1) -= xy * yIdx;
        C.coeffRef (2) -= xz * yIdx;
        C.coeffRef (3) -= point.x * yIdx;

        C.coeffRef (5) -= yy * yIdx;
        C.coeffRef (6) -= yz * yIdx;
        C.coeffRef (7) -= point.y * yIdx;

        C.coeffRef (10) -= zz * yIdx;
        C.coeffRef (11) -= point.z * yIdx;

        C.coeffRef (15) -= yIdx;

        D.coeffRef (0) += xx * xx_yy;
        D.coeffRef (1) += xy * xx_yy;
        D.coeffRef (2) += xz * xx_yy;
        D.coeffRef (3) += point.x * xx_yy;

        D.coeffRef (5) += yy * xx_yy;
        D.coeffRef (6) += yz * xx_yy;
        D.coeffRef (7) += point.y * xx_yy;

        D.coeffRef (10) += zz * xx_yy;
        D.coeffRef (11) += point.z * xx_yy;

        D.coeffRef (15) += xx_yy;
      }
    }
  }

  makeSymmetric(A);
  makeSymmetric(B);
  makeSymmetric(C);
  makeSymmetric(D);

  Eigen::Matrix<Scalar, 12, 12> X = Eigen::Matrix<Scalar, 12, 12>::Zero ();
  X.topLeftCorner<4,4> () = A;
  X.block<4,4> (0, 8) = B;
//.........这里部分代码省略.........

template <typename PointT> double 
pcl::estimateProjectionMatrix (
    typename pcl::PointCloud<PointT>::ConstPtr cloud, 
    Eigen::Matrix<float, 3, 4, Eigen::RowMajor>& projection_matrix, 
    const std::vector<int>& indices)
{
  // internally we calculate with double but store the result into float matrices.
  typedef double Scalar;
  projection_matrix.setZero ();
  if (cloud->height == 1 || cloud->width == 1)
  {
    PCL_ERROR ("[pcl::estimateProjectionMatrix] Input dataset is not organized!\n");
    return (-1.0);
  }
  
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> A = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> B = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> C = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();
  Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor> D = Eigen::Matrix<Scalar, 4, 4, Eigen::RowMajor>::Zero ();

  pcl::ConstCloudIterator <PointT> pointIt (*cloud, indices);
  
  while (pointIt)
  {
    unsigned yIdx = pointIt.getCurrentPointIndex () / cloud->width;
    unsigned xIdx = pointIt.getCurrentPointIndex () % cloud->width;
    
    const PointT& point = *pointIt;
    if (pcl_isfinite (point.x))
    {
      Scalar xx = point.x * point.x;
      Scalar xy = point.x * point.y;
      Scalar xz = point.x * point.z;
      Scalar yy = point.y * point.y;
      Scalar yz = point.y * point.z;
      Scalar zz = point.z * point.z;
      Scalar xx_yy = xIdx * xIdx + yIdx * yIdx;

      A.coeffRef (0) += xx;
      A.coeffRef (1) += xy;
      A.coeffRef (2) += xz;
      A.coeffRef (3) += point.x;

      A.coeffRef (5) += yy;
      A.coeffRef (6) += yz;
      A.coeffRef (7) += point.y;

      A.coeffRef (10) += zz;
      A.coeffRef (11) += point.z;
      A.coeffRef (15) += 1.0;

      B.coeffRef (0) -= xx * xIdx;
      B.coeffRef (1) -= xy * xIdx;
      B.coeffRef (2) -= xz * xIdx;
      B.coeffRef (3) -= point.x * static_cast<double>(xIdx);

      B.coeffRef (5) -= yy * xIdx;
      B.coeffRef (6) -= yz * xIdx;
      B.coeffRef (7) -= point.y * static_cast<double>(xIdx);

      B.coeffRef (10) -= zz * xIdx;
      B.coeffRef (11) -= point.z * static_cast<double>(xIdx);

      B.coeffRef (15) -= xIdx;

      C.coeffRef (0) -= xx * yIdx;
      C.coeffRef (1) -= xy * yIdx;
      C.coeffRef (2) -= xz * yIdx;
      C.coeffRef (3) -= point.x * static_cast<double>(yIdx);

      C.coeffRef (5) -= yy * yIdx;
      C.coeffRef (6) -= yz * yIdx;
      C.coeffRef (7) -= point.y * static_cast<double>(yIdx);

      C.coeffRef (10) -= zz * yIdx;
      C.coeffRef (11) -= point.z * static_cast<double>(yIdx);

      C.coeffRef (15) -= yIdx;

      D.coeffRef (0) += xx * xx_yy;
      D.coeffRef (1) += xy * xx_yy;
      D.coeffRef (2) += xz * xx_yy;
      D.coeffRef (3) += point.x * xx_yy;

      D.coeffRef (5) += yy * xx_yy;
      D.coeffRef (6) += yz * xx_yy;
      D.coeffRef (7) += point.y * xx_yy;

      D.coeffRef (10) += zz * xx_yy;
      D.coeffRef (11) += point.z * xx_yy;

      D.coeffRef (15) += xx_yy;
    }
    
    ++pointIt;
  } // while  
  
  pcl::common::internal::makeSymmetric (A);
  pcl::common::internal::makeSymmetric (B);
  pcl::common::internal::makeSymmetric (C);
//.........这里部分代码省略.........