コード例 #1
0
ファイル: Math.cpp プロジェクト: CrazyHeex/woo
bool MatrixXr_pseudoInverse(const MatrixXr &a, MatrixXr &a_pinv, double epsilon) {

    // see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
    if ( a.rows()<a.cols() ) return false;

    // SVD
    Eigen::JacobiSVD<MatrixXr> svdA;
    svdA.compute(a,Eigen::ComputeThinU|Eigen::ComputeThinV);
    MatrixXr vSingular = svdA.singularValues();

    // Build a diagonal matrix with the Inverted Singular values
    // The pseudo inverted singular matrix is easy to compute :
    // is formed by replacing every nonzero entry by its reciprocal (inversing).
    VectorXr vPseudoInvertedSingular(svdA.matrixV().cols(),1);

    for (int iRow =0; iRow<vSingular.rows(); iRow++) {
        if(fabs(vSingular(iRow))<=epsilon) vPseudoInvertedSingular(iRow,0)=0.;
        else vPseudoInvertedSingular(iRow,0)=1./vSingular(iRow);
    }

    // A little optimization here
    MatrixXr mAdjointU = svdA.matrixU().adjoint().block(0,0,vSingular.rows(),svdA.matrixU().adjoint().cols());

    // Pseudo-Inversion : V * S * U'
    a_pinv = (svdA.matrixV() *  vPseudoInvertedSingular.asDiagonal()) * mAdjointU;

    return true;
}
コード例 #2
0
void projectorFromSvd(const Eigen::MatrixXd& jac,
                      Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
                      Eigen::VectorXd& svdSingular,
                      Eigen::MatrixXd& preResult,
                      Eigen::MatrixXd& result,
                      double epsilon=std::numeric_limits<double>::epsilon(),
                      double minTol=1e-8)
{
  // we are force to compute the Full matrix because of
  // the nullspace matrix computation
  svd.compute(jac, Eigen::ComputeFullU | Eigen::ComputeFullV);

  double tolerance =
      epsilon*double(std::max(jac.cols(), jac.rows()))*
      std::abs(svd.singularValues()[0]);
  tolerance = std::max(tolerance, minTol);

  svdSingular.setOnes();
  for(int i = 0; i < svd.singularValues().rows(); ++i)
  {
    svdSingular[i] = svd.singularValues()[i] > tolerance ? 0. : 1.;
  }

  preResult.noalias() = svd.matrixV()*svdSingular.asDiagonal();
  result.noalias() = preResult*svd.matrixV().adjoint();
}
コード例 #3
0
ファイル: linearTF_solver.hpp プロジェクト: mruan/range_calib
  // Assume t = double[3], q = double[4]
  void EstimateTfSVD(double* t, double* q)
  {
    // Assemble the correlation matrix H = target * reference'
    Eigen::Matrix3d H = (cloud_tgt_demean * cloud_ref_demean.transpose ()).topLeftCorner<3, 3>();

    // Compute the Singular Value Decomposition
    Eigen::JacobiSVD<Eigen::Matrix3d> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3d u = svd.matrixU ();
    Eigen::Matrix3d v = svd.matrixV ();

    // Compute R = V * U'
    if (u.determinant () * v.determinant () < 0)
      {
	for (int i = 0; i < 3; ++i)
	  v (i, 2) *= -1;
      }

    //    std::cout<< "centroid_src: "<<centroid_src(0) <<" "<< centroid_src(1) <<" "<< centroid_src(2) << " "<< centroid_src(3)<<std::endl;
    //    std::cout<< "centroid_tgt: "<<centroid_tgt(0) <<" "<< centroid_tgt(1) <<" "<< centroid_tgt(2) << " "<< centroid_tgt(3)<<std::endl;
    
    Eigen::Matrix3d R = v * u.transpose ();

    const Eigen::Vector3d Rc (R * centroid_tgt.head<3> ());
    Eigen::Vector3d T = centroid_ref.head<3> () - Rc;

    // Make sure these memory locations are valid
    assert(t != NULL && q!=NULL);
    Eigen::Quaterniond Q(R);
    t[0] = T(0);  t[1] = T(1);  t[2] = T(2);
    q[0] = Q.w(); q[1] = Q.x(); q[2] = Q.y(); q[3] = Q.z();
  }
コード例 #4
0
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_src_demean,
    const Eigen::Matrix<Scalar, 4, 1> &centroid_src,
    const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
    const Eigen::Matrix<Scalar, 4, 1> &centroid_tgt,
    Matrix4 &transformation_matrix) const
{
  transformation_matrix.setIdentity ();

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner (3, 3);

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();

  // Return the correct transformation
  transformation_matrix.topLeftCorner (3, 3) = R;
  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.head (3));
  transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.head (3) - Rc;
}
コード例 #5
0
template <typename PointSource, typename PointTarget> void
pcl::registration::TransformationEstimationSVD<PointSource, PointTarget>::getTransformationFromCorrelation (
    const Eigen::MatrixXf &cloud_src_demean,
    const Eigen::Vector4f &centroid_src,
    const Eigen::MatrixXf &cloud_tgt_demean,
    const Eigen::Vector4f &centroid_tgt,
    Eigen::Matrix4f &transformation_matrix)
{
  transformation_matrix.setIdentity ();

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3f u = svd.matrixU ();
  Eigen::Matrix3f v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix3f R = v * u.transpose ();

  // Return the correct transformation
  transformation_matrix.topLeftCorner<3, 3> () = R;
  Eigen::Vector3f Rc = R * centroid_src.head<3> ();
  transformation_matrix.block <3, 1> (0, 3) = centroid_tgt.head<3> () - Rc;
}
コード例 #6
0
void pose_estimation_3d3d (
    const vector<Point3f>& pts1,
    const vector<Point3f>& pts2,
    Mat& R, Mat& t
)
{
    Point3f p1, p2;     // center of mass
    int N = pts1.size();
    for ( int i=0; i<N; i++ )
    {
        p1 += pts1[i];
        p2 += pts2[i];
    }
    p1 = Point3f( Vec3f(p1) /  N);
    p2 = Point3f( Vec3f(p2) / N);
    vector<Point3f>     q1 ( N ), q2 ( N ); // remove the center
    for ( int i=0; i<N; i++ )
    {
        q1[i] = pts1[i] - p1;
        q2[i] = pts2[i] - p2;
    }

    // compute q1*q2^T
    Eigen::Matrix3d W = Eigen::Matrix3d::Zero();
    for ( int i=0; i<N; i++ )
    {
        W += Eigen::Vector3d ( q1[i].x, q1[i].y, q1[i].z ) * Eigen::Vector3d ( q2[i].x, q2[i].y, q2[i].z ).transpose();
    }
    cout<<"W="<<W<<endl;

    // SVD on W
    Eigen::JacobiSVD<Eigen::Matrix3d> svd ( W, Eigen::ComputeFullU|Eigen::ComputeFullV );
    Eigen::Matrix3d U = svd.matrixU();
    Eigen::Matrix3d V = svd.matrixV();
    
    if (U.determinant() * V.determinant() < 0)
	{
        for (int x = 0; x < 3; ++x)
        {
            U(x, 2) *= -1;
        }
	}
    
    cout<<"U="<<U<<endl;
    cout<<"V="<<V<<endl;

    Eigen::Matrix3d R_ = U* ( V.transpose() );
    Eigen::Vector3d t_ = Eigen::Vector3d ( p1.x, p1.y, p1.z ) - R_ * Eigen::Vector3d ( p2.x, p2.y, p2.z );

    // convert to cv::Mat
    R = ( Mat_<double> ( 3,3 ) <<
          R_ ( 0,0 ), R_ ( 0,1 ), R_ ( 0,2 ),
          R_ ( 1,0 ), R_ ( 1,1 ), R_ ( 1,2 ),
          R_ ( 2,0 ), R_ ( 2,1 ), R_ ( 2,2 )
        );
    t = ( Mat_<double> ( 3,1 ) << t_ ( 0,0 ), t_ ( 1,0 ), t_ ( 2,0 ) );
}
コード例 #7
0
ファイル: eigen_pinv.hpp プロジェクト: GT-RAIL/carl_moveit
// Derived from code by Yohann Solaro ( http://listengine.tuxfamily.org/lists.tuxfamily.org/eigen/2010/01/msg00187.html )
// see : http://en.wikipedia.org/wiki/Moore-Penrose_pseudoinverse#The_general_case_and_the_SVD_method
Eigen::MatrixXd pinv( const Eigen::MatrixXd &b, double rcond )
{
  // TODO: Figure out why it wants fewer rows than columns
  // if ( a.rows()<a.cols() )
  // return false;
  bool flip = false;
  Eigen::MatrixXd a;
  if( a.rows() < a.cols() )
  {
    a = b.transpose();
    flip = true;
  }
  else
    a = b;
  // SVD
  Eigen::JacobiSVD<Eigen::MatrixXd> svdA;
  svdA.compute( a, Eigen::ComputeFullU | Eigen::ComputeThinV );
  Eigen::JacobiSVD<Eigen::MatrixXd>::SingularValuesType vSingular = svdA.singularValues();
  // Build a diagonal matrix with the Inverted Singular values
  // The pseudo inverted singular matrix is easy to compute :
  // is formed by replacing every nonzero entry by its reciprocal (inversing).
  Eigen::VectorXd vPseudoInvertedSingular( svdA.matrixV().cols() );

  for (int iRow=0; iRow<vSingular.rows(); iRow++)
  {
    if ( fabs(vSingular(iRow)) <= rcond )
    {
      vPseudoInvertedSingular(iRow)=0.;
    }
    else
      vPseudoInvertedSingular(iRow)=1./vSingular(iRow);
  }
  // A little optimization here
  Eigen::MatrixXd mAdjointU = svdA.matrixU().adjoint().block( 0, 0, vSingular.rows(), svdA.matrixU().adjoint().cols() );
  // Pseudo-Inversion : V * S * U'
  Eigen::MatrixXd a_pinv = (svdA.matrixV() * vPseudoInvertedSingular.asDiagonal()) * mAdjointU;
  if( flip )
  {
    a = a.transpose();
    a_pinv = a_pinv.transpose();
  }
  return a_pinv;
}
コード例 #8
0
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
    const Eigen::MatrixXf &cloud_src_demean,
    const Eigen::Vector4f &centroid_src,
    const Eigen::MatrixXf &cloud_tgt_demean,
    const Eigen::Vector4f &centroid_tgt,
    Matrix4 &transformation_matrix) const
{
  transformation_matrix.setIdentity ();

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix<Scalar, 3, 3> H = (cloud_src_demean.cast<Scalar> () * cloud_tgt_demean.cast<Scalar> ().transpose ()).topLeftCorner (3, 3);

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix<Scalar, 3, 3> > svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix<Scalar, 3, 3> u = svd.matrixU ();
  Eigen::Matrix<Scalar, 3, 3> v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix<Scalar, 3, 3> R = v * u.transpose ();

  // rotated cloud
  Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> src_ = R * cloud_src_demean.cast<Scalar> ();
  
  float scale1, scale2;
  double sum_ss = 0.0f, sum_tt = 0.0f, sum_tt_ = 0.0f;
  for (unsigned corrIdx = 0; corrIdx < cloud_src_demean.cols (); ++corrIdx)
  {
    sum_ss += cloud_src_demean (0, corrIdx) * cloud_src_demean (0, corrIdx);
    sum_ss += cloud_src_demean (1, corrIdx) * cloud_src_demean (1, corrIdx);
    sum_ss += cloud_src_demean (2, corrIdx) * cloud_src_demean (2, corrIdx);
    
    sum_tt += cloud_tgt_demean (0, corrIdx) * cloud_tgt_demean (0, corrIdx);
    sum_tt += cloud_tgt_demean (1, corrIdx) * cloud_tgt_demean (1, corrIdx);
    sum_tt += cloud_tgt_demean (2, corrIdx) * cloud_tgt_demean (2, corrIdx);
    
    sum_tt_ += cloud_tgt_demean (0, corrIdx) * src_ (0, corrIdx);
    sum_tt_ += cloud_tgt_demean (1, corrIdx) * src_ (1, corrIdx);
    sum_tt_ += cloud_tgt_demean (2, corrIdx) * src_ (2, corrIdx);
  }
  
  scale1 = sqrt (sum_tt / sum_ss);
  scale2 = sum_tt_ / sum_ss;
  float scale = scale2;
  transformation_matrix.topLeftCorner (3, 3) = scale * R;
  const Eigen::Matrix<Scalar, 3, 1> Rc (R * centroid_src.cast<Scalar> ().head (3));
  transformation_matrix.block (0, 3, 3, 1) = centroid_tgt.cast<Scalar> (). head (3) - Rc;
}
コード例 #9
0
/**
 * estimateRigidTransformationSVD
 */
void RigidTransformationRANSAC::estimateRigidTransformationSVD(
      const std::vector<Eigen::Vector3f > &srcPts,
      const std::vector<int> &srcIndices,
      const std::vector<Eigen::Vector3f > &tgtPts,
      const std::vector<int> &tgtIndices,
      Eigen::Matrix4f &transform)
{
  Eigen::Vector3f srcCentroid, tgtCentroid;

  // compute the centroids of source, target
  ComputeCentroid (srcPts, srcIndices, srcCentroid);
  ComputeCentroid (tgtPts, tgtIndices, tgtCentroid);

  // Subtract the centroids from source, target
  Eigen::MatrixXf srcPtsDemean;
  DemeanPoints(srcPts, srcIndices, srcCentroid, srcPtsDemean);

  Eigen::MatrixXf tgtPtsDemean;
  DemeanPoints(tgtPts, tgtIndices, tgtCentroid, tgtPtsDemean);

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix3f H = (srcPtsDemean * tgtPtsDemean.transpose ()).topLeftCorner<3, 3>();

  // Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3f u = svd.matrixU ();
  Eigen::Matrix3f v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  // Return the transformation
  Eigen::Matrix3f R = v * u.transpose ();
  Eigen::Vector3f t = tgtCentroid - R * srcCentroid;

  // set rotation
  transform.block(0,0,3,3) = R;
  // set translation
  transform.block(0,3,3,1) = t;
  transform.block(3, 0, 1, 3).setZero();  
  transform(3,3) = 1.;
}
コード例 #10
0
template <typename PointT> void
pcl::SampleConsensusModelRegistration<PointT>::estimateRigidTransformationSVD (
    const pcl::PointCloud<PointT> &cloud_src, 
    const std::vector<int> &indices_src, 
    const pcl::PointCloud<PointT> &cloud_tgt,
    const std::vector<int> &indices_tgt, 
    Eigen::VectorXf &transform)
{
  transform.resize (16);
  Eigen::Vector4f centroid_src, centroid_tgt;
  // Estimate the centroids of source, target
  compute3DCentroid (cloud_src, indices_src, centroid_src);
  compute3DCentroid (cloud_tgt, indices_tgt, centroid_tgt);

  // Subtract the centroids from source, target
  Eigen::MatrixXf cloud_src_demean;
  demeanPointCloud (cloud_src, indices_src, centroid_src, cloud_src_demean);

  Eigen::MatrixXf cloud_tgt_demean;
  demeanPointCloud (cloud_tgt, indices_tgt, centroid_tgt, cloud_tgt_demean);

  // Assemble the correlation matrix H = source * target'
  Eigen::Matrix3f H = (cloud_src_demean * cloud_tgt_demean.transpose ()).topLeftCorner<3, 3>();

  // Compute the Singular Value Decomposition
  Eigen::JacobiSVD<Eigen::Matrix3f> svd (H, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::Matrix3f u = svd.matrixU ();
  Eigen::Matrix3f v = svd.matrixV ();

  // Compute R = V * U'
  if (u.determinant () * v.determinant () < 0)
  {
    for (int x = 0; x < 3; ++x)
      v (x, 2) *= -1;
  }

  Eigen::Matrix3f R = v * u.transpose ();

  // Return the correct transformation
  transform.segment<3> (0) = R.row (0); transform[12]  = 0;
  transform.segment<3> (4) = R.row (1); transform[13]  = 0;
  transform.segment<3> (8) = R.row (2); transform[14] = 0;

  Eigen::Vector3f t = centroid_tgt.head<3> () - R * centroid_src.head<3> ();
  transform[3] = t[0]; transform[7] = t[1]; transform[11] = t[2]; transform[15] = 1.0;
}
コード例 #11
0
int main() {

  std::ifstream file;
  file.open("SVD_benchmark");
  if (!file) 
  {
    CGAL_TRACE_STREAM << "Error loading file!\n";
    return 0;
  }

  int ite = 200000;
  Eigen::JacobiSVD<Eigen::Matrix3d> svd;
  Eigen::Matrix3d u, v, cov, r;         
  Eigen::Vector3d w;   

  int matrix_idx = rand()%200;
  for (int i = 0; i < matrix_idx; i++)
  {
    for (int j = 0; j < 3; j++)
    {
      for (int k = 0; k < 3; k++)
      {
        file >> cov(j, k);
      }
    }
  }


  CGAL::Timer task_timer; 

  CGAL_TRACE_STREAM << "Start SVD decomposition...";
  task_timer.start();
  for (int i = 0; i < ite; i++)
  {
    
    svd.compute( cov, Eigen::ComputeFullU | Eigen::ComputeFullV );
    u = svd.matrixU(); v = svd.matrixV(); w = svd.singularValues();
    r = v*u.transpose();
  }
  task_timer.stop();
  file.close();

  CGAL_TRACE_STREAM << "done: " << task_timer.time() << "s\n";

  return 0;
}
コード例 #12
0
const CPoint3DCAMERA CMiniVisionToolbox::getPointStereoLinearTriangulationSVDDLT( const cv::Point2d& p_ptPointLEFT, const cv::Point2d& p_ptPointRIGHT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionLEFT, const Eigen::Matrix< double, 3, 4 >& p_matProjectionRIGHT )
{
    //ds A matrix for system: A*X=0
    Eigen::Matrix4d matAHomogeneous;

    matAHomogeneous.row(0) = p_ptPointLEFT.x*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(0);
    matAHomogeneous.row(1) = p_ptPointLEFT.y*p_matProjectionLEFT.row(2)-p_matProjectionLEFT.row(1);
    matAHomogeneous.row(2) = p_ptPointRIGHT.x*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(0);
    matAHomogeneous.row(3) = p_ptPointRIGHT.y*p_matProjectionRIGHT.row(2)-p_matProjectionRIGHT.row(1);

    //ds solve system subject to ||A*x||=0 ||x||=1
    const Eigen::JacobiSVD< Eigen::Matrix4d > matSVD( matAHomogeneous, Eigen::ComputeFullU | Eigen::ComputeFullV );

    //ds solution x is the last column of V
    const Eigen::Vector4d vecX( matSVD.matrixV( ).col( 3 ) );

    return vecX.head( 3 )/vecX(3);
}
コード例 #13
0
ファイル: cv_tools.hpp プロジェクト: mikewiltero/Sub8
bool pseudoInverse(
    const _Matrix_Type_ &a, _Matrix_Type_ &result,
    double epsilon =
        std::numeric_limits<typename _Matrix_Type_::Scalar>::epsilon()) {
  if (a.rows() < a.cols())
    return false;

  Eigen::JacobiSVD<_Matrix_Type_> svd = a.jacobiSvd();

  typename _Matrix_Type_::Scalar tolerance =
      epsilon * std::max(a.cols(), a.rows()) *
      svd.singularValues().array().abs().maxCoeff();

  result = svd.matrixV() *
           _Matrix_Type_(
               _Matrix_Type_((svd.singularValues().array().abs() > tolerance)
                                 .select(svd.singularValues().array().inverse(),
                                         0)).diagonal()) *
           svd.matrixU().adjoint();
}
コード例 #14
0
ファイル: board.hpp プロジェクト: 2php/pcl
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::planeFitting (
                                                                                     Eigen::Matrix<float,
                                                                                         Eigen::Dynamic, 3> const &points,
                                                                                     Eigen::Vector3f &center,
                                                                                     Eigen::Vector3f &norm)
{
  // -----------------------------------------------------
  // Plane Fitting using Singular Value Decomposition (SVD)
  // -----------------------------------------------------

  int n_points = static_cast<int> (points.rows ());
  if (n_points == 0)
  {
    return;
  }

  //find the center by averaging the points positions
  center.setZero ();

  for (int i = 0; i < n_points; ++i)
  {
    center += points.row (i);
  }

  center /= static_cast<float> (n_points);

  //copy points - average (center)
  Eigen::Matrix<float, Eigen::Dynamic, 3> A (n_points, 3); //PointData
  for (int i = 0; i < n_points; ++i)
  {
    A (i, 0) = points (i, 0) - center.x ();
    A (i, 1) = points (i, 1) - center.y ();
    A (i, 2) = points (i, 2) - center.z ();
  }

  Eigen::JacobiSVD<Eigen::MatrixXf> svd (A, Eigen::ComputeFullV);
  norm = svd.matrixV ().col (2);
}
コード例 #15
0
inline Eigen::Affine3f
pcl::TransformationFromCorrespondences::getTransformation ()
{
  //Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
  const Eigen::Matrix<float, 3, 3>& u = svd.matrixU(),
                                   & v = svd.matrixV();
  Eigen::Matrix<float, 3, 3> s;
  s.setIdentity();
  if (u.determinant()*v.determinant() < 0.0f)
    s(2,2) = -1.0f;
  
  Eigen::Matrix<float, 3, 3> r = u * s * v.transpose();
  Eigen::Vector3f t = mean2_ - r*mean1_;
  
  Eigen::Affine3f ret;
  ret(0,0)=r(0,0); ret(0,1)=r(0,1); ret(0,2)=r(0,2); ret(0,3)=t(0);
  ret(1,0)=r(1,0); ret(1,1)=r(1,1); ret(1,2)=r(1,2); ret(1,3)=t(1);
  ret(2,0)=r(2,0); ret(2,1)=r(2,1); ret(2,2)=r(2,2); ret(2,3)=t(2);
  ret(3,0)=0.0f;   ret(3,1)=0.0f;   ret(3,2)=0.0f;   ret(3,3)=1.0f;
  
  return (ret);
}
コード例 #16
0
void pseudoInverse(const Eigen::MatrixXd& jac,
                   Eigen::JacobiSVD<Eigen::MatrixXd>& svd,
                   Eigen::VectorXd& svdSingular,
                   Eigen::MatrixXd& prePseudoInv,
                   Eigen::MatrixXd& result,
                   double epsilon=std::numeric_limits<double>::epsilon(),
                   double minTol=1e-8)
{
  svd.compute(jac, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // singular values are sorted in decreasing order
  // so the first one is the max one
  double tolerance =
      epsilon*double(std::max(jac.cols(), jac.rows()))*
      std::abs(svd.singularValues()[0]);
  tolerance = std::max(tolerance, minTol);

  svdSingular = ((svd.singularValues().array().abs() > tolerance).
      select(svd.singularValues().array().inverse(), 0.));

  prePseudoInv.noalias() = svd.matrixV()*svdSingular.asDiagonal();
  result.noalias() = prePseudoInv*svd.matrixU().adjoint();
}
コード例 #17
0
// typename DerivedA::Scalar
void pseudo_inverse_svd(const Eigen::MatrixBase<DerivedA>& M,
  Eigen::MatrixBase<OutputMatrixType>& Minv,
  typename DerivedA::Scalar epsilon = 1e-6)//std::numeric_limits<typename DerivedA::Scalar>::epsilon())
{
  // CONTROLIT_INFO << "Method called!\n  epsilon = " << epsilon << ", M = \n" << M;

  // Ensure matrix Minv has the correct size.  Its size should be equal to M.transpose().
  assert_msg(M.rows() == Minv.cols(), "Minv has invalid number of columns.  Expected " << M.rows() << " got " << Minv.cols());
  assert_msg(M.cols() == Minv.rows(), "Minv has invalid number of rows.  Expected " << M.cols() << " got " << Minv.rows());

  // According to Eigen documentation, "If the input matrix has inf or nan coefficients, the result of the
  // computation is undefined, but the computation is guaranteed to terminate in finite (and reasonable) time."
  Eigen::JacobiSVD<DerivedA> svd = M.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV);

  // Get the max singular value
  typename DerivedA::Scalar maxSingularValue = svd.singularValues().array().abs().maxCoeff();

  // Use Minv to temporarily hold sigma
  Minv.setZero();

  typename DerivedA::Scalar tolerance = 0;

  // Only compute sigma if the max singular value is greater than zero.
  if (maxSingularValue > epsilon)
  {
    tolerance = epsilon * std::max(M.cols(), M.rows()) * maxSingularValue;

    // For each singular value of matrix M's SVD decomposition, check if it is greater than
    // the tolerance value.  If it is, save 1/(singular value) in the sigma vector.
    // Otherwise save zero in the sigma vector.
    DerivedA sigmaVector = DerivedA( (svd.singularValues().array().abs() > tolerance).select(svd.singularValues().array().inverse(), 0) );
    // DerivedA zeroSVs = DerivedA( (svd.singularValues().array().abs() <= tolerance).select(svd.singularValues().array().inverse(), 0) );

    // CONTROLIT_INFO << "epsilon: " << epsilon << ", std::max(M.cols(), M.rows()): " << std::max(M.cols(), M.rows()) << ", maxSingularValue: " << maxSingularValue << ", tolerance: " << tolerance;
    // CONTROLIT_INFO << "sigmaVector = " << sigmaVector.transpose();
    // CONTROLIT_INFO << "zeroSigmaVector : "<< zeroSVs.transpose();

    Minv.block(0, 0, sigmaVector.rows(), sigmaVector.rows()) = sigmaVector.asDiagonal();
  }

  // Double check to make sure the matrices have the correct dimensions
  assert_msg(svd.matrixV().cols() == Minv.rows(),
    "Matrix dimension mismatch, svd.matrixV().cols() = " << svd.matrixV().cols() << ", Minv.rows() = " << Minv.rows() << ".");
  assert_msg(Minv.cols() == svd.matrixU().adjoint().rows(),
    "Matrix dimension mismatch, Minv.cols() = " << Minv.cols() << ", svd.matrixU().adjoint().rows() = " << svd.matrixU().adjoint().rows() << ".");

  Minv = svd.matrixV() *
         Minv *
         svd.matrixU().adjoint(); // take the transpose of matrix U

  // CONTROLIT_INFO << "Done method call! Minv = " << Minv;

  // typename DerivedA::Scalar errorNorm = std::abs((M * Minv - DerivedA::Identity(M.rows(), Minv.cols())).norm());

  // if (tolerance != 0 && errorNorm > tolerance * 10)
  // {
  //   CONTROLIT_WARN << "Problems computing pseudoinverse.  Perhaps the tolerance is too high?\n"
  //     << "  - epsilon: " << epsilon << "\n"
  //     << "  - tolerance: " << tolerance << "\n"
  //     << "  - maxSingularValue: " << maxSingularValue << "\n"
  //     << "  - errorNorm: " << errorNorm << "\n"
  //     << "  - M:\n" << M << "\n"
  //     << "  - Minv:\n" << Minv;
  // }

  // return errorNorm;
}
コード例 #18
0
ファイル: min_quad_dense.cpp プロジェクト: bbrrck/libigl
IGL_INLINE void igl::min_quad_dense_precompute(
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
  const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& Aeq,    
  const bool use_lu_decomposition,
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& S)
{
  typedef Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> Mat;
        // This threshold seems to matter a lot but I'm not sure how to
        // set it
  const T treshold = igl::FLOAT_EPS;
  //const T treshold = igl::DOUBLE_EPS;

  const int n = A.rows();
  assert(A.cols() == n);
  const int m = Aeq.rows();
  assert(Aeq.cols() == n);

  // Lagrange multipliers method:
  Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> LM(n + m, n + m);
  LM.block(0, 0, n, n) = A;
  LM.block(0, n, n, m) = Aeq.transpose();
  LM.block(n, 0, m, n) = Aeq;
  LM.block(n, n, m, m).setZero();

  Mat LMpinv;
  if(use_lu_decomposition)
  {
    // if LM is close to singular, use at your own risk :)
    LMpinv = LM.inverse();
  }else
  {
    // use SVD
    typedef Eigen::Matrix<T, Eigen::Dynamic, 1> Vec; 
    Vec singValues;
    Eigen::JacobiSVD<Mat> svd;
    svd.compute(LM, Eigen::ComputeFullU | Eigen::ComputeFullV );
    const Mat& u = svd.matrixU();
    const Mat& v = svd.matrixV();
    const Vec& singVals = svd.singularValues();

    Vec pi_singVals(n + m);
    int zeroed = 0;
    for (int i=0; i<n + m; i++)
    {
      T sv = singVals(i, 0);
      assert(sv >= 0);      
                 // printf("sv: %lg ? %lg\n",(double) sv,(double)treshold);
      if (sv > treshold) pi_singVals(i, 0) = T(1) / sv;
      else 
      {
        pi_singVals(i, 0) = T(0);
        zeroed++;
      }
    }

    printf("min_quad_dense_precompute: %i singular values zeroed (threshold = %e)\n", zeroed, treshold);
    Eigen::DiagonalMatrix<T, Eigen::Dynamic> pi_diag(pi_singVals);

    LMpinv = v * pi_diag * u.transpose();
  }
  S = LMpinv.block(0, 0, n, n + m);

  //// debug:
  //mlinit(&g_pEngine);
  //
  //mlsetmatrix(&g_pEngine, "A", A);
  //mlsetmatrix(&g_pEngine, "Aeq", Aeq);
  //mlsetmatrix(&g_pEngine, "LM", LM);
  //mlsetmatrix(&g_pEngine, "u", u);
  //mlsetmatrix(&g_pEngine, "v", v);
  //MatrixXd svMat = singVals;
  //mlsetmatrix(&g_pEngine, "singVals", svMat);
  //mlsetmatrix(&g_pEngine, "LMpinv", LMpinv);
  //mlsetmatrix(&g_pEngine, "S", S);

  //int hu = 1;
}
コード例 #19
0
ファイル: our_cvfh.hpp プロジェクト: 5irius/pcl
template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::sgurf (Eigen::Vector3f & centroid, Eigen::Vector3f & normal_centroid,
                                                               PointInTPtr & processed, std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transformations,
                                                               PointInTPtr & grid, pcl::PointIndices & indices)
{

  Eigen::Vector3f plane_normal;
  plane_normal[0] = -centroid[0];
  plane_normal[1] = -centroid[1];
  plane_normal[2] = -centroid[2];
  Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
  plane_normal.normalize ();
  Eigen::Vector3f axis = plane_normal.cross (z_vector);
  double rotation = -asin (axis.norm ());
  axis.normalize ();

  Eigen::Affine3f transformPC (Eigen::AngleAxisf (static_cast<float> (rotation), axis));

  grid->points.resize (processed->points.size ());
  for (size_t k = 0; k < processed->points.size (); k++)
    grid->points[k].getVector4fMap () = processed->points[k].getVector4fMap ();

  pcl::transformPointCloud (*grid, *grid, transformPC);

  Eigen::Vector4f centroid4f (centroid[0], centroid[1], centroid[2], 0);
  Eigen::Vector4f normal_centroid4f (normal_centroid[0], normal_centroid[1], normal_centroid[2], 0);

  centroid4f = transformPC * centroid4f;
  normal_centroid4f = transformPC * normal_centroid4f;

  Eigen::Vector3f centroid3f (centroid4f[0], centroid4f[1], centroid4f[2]);

  Eigen::Vector4f farthest_away;
  pcl::getMaxDistance (*grid, indices.indices, centroid4f, farthest_away);
  farthest_away[3] = 0;

  float max_dist = (farthest_away - centroid4f).norm ();

  pcl::demeanPointCloud (*grid, centroid4f, *grid);

  Eigen::Matrix4f center_mat;
  center_mat.setIdentity (4, 4);
  center_mat (0, 3) = -centroid4f[0];
  center_mat (1, 3) = -centroid4f[1];
  center_mat (2, 3) = -centroid4f[2];

  Eigen::Matrix3f scatter;
  scatter.setZero ();
  float sum_w = 0.f;

  //for (int k = 0; k < static_cast<intgrid->points[k].getVector3fMap ();> (grid->points.size ()); k++)
  for (int k = 0; k < static_cast<int> (indices.indices.size ()); k++)
  {
    Eigen::Vector3f pvector = grid->points[indices.indices[k]].getVector3fMap ();
    float d_k = (pvector).norm ();
    float w = (max_dist - d_k);
    Eigen::Vector3f diff = (pvector);
    Eigen::Matrix3f mat = diff * diff.transpose ();
    scatter = scatter + mat * w;
    sum_w += w;
  }

  scatter /= sum_w;

  Eigen::JacobiSVD <Eigen::MatrixXf> svd (scatter, Eigen::ComputeFullV);
  Eigen::Vector3f evx = svd.matrixV ().col (0);
  Eigen::Vector3f evy = svd.matrixV ().col (1);
  Eigen::Vector3f evz = svd.matrixV ().col (2);
  Eigen::Vector3f evxminus = evx * -1;
  Eigen::Vector3f evyminus = evy * -1;
  Eigen::Vector3f evzminus = evz * -1;

  float s_xplus, s_xminus, s_yplus, s_yminus;
  s_xplus = s_xminus = s_yplus = s_yminus = 0.f;

  //disambiguate rf using all points
  for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
  {
    Eigen::Vector3f pvector = grid->points[k].getVector3fMap ();
    float dist_x, dist_y;
    dist_x = std::abs (evx.dot (pvector));
    dist_y = std::abs (evy.dot (pvector));

    if ((pvector).dot (evx) >= 0)
      s_xplus += dist_x;
    else
      s_xminus += dist_x;

    if ((pvector).dot (evy) >= 0)
      s_yplus += dist_y;
    else
      s_yminus += dist_y;

  }

  if (s_xplus < s_xminus)
    evx = evxminus;

  if (s_yplus < s_yminus)
    evy = evyminus;

  //select the axis that could be disambiguated more easily
  float fx, fy;
  float max_x = static_cast<float> (std::max (s_xplus, s_xminus));
  float min_x = static_cast<float> (std::min (s_xplus, s_xminus));
  float max_y = static_cast<float> (std::max (s_yplus, s_yminus));
  float min_y = static_cast<float> (std::min (s_yplus, s_yminus));

  fx = (min_x / max_x);
  fy = (min_y / max_y);

  Eigen::Vector3f normal3f = Eigen::Vector3f (normal_centroid4f[0], normal_centroid4f[1], normal_centroid4f[2]);
  if (normal3f.dot (evz) < 0)
    evz = evzminus;

  //if fx/y close to 1, it was hard to disambiguate
  //what if both are equally easy or difficult to disambiguate, namely fy == fx or very close

  float max_axis = std::max (fx, fy);
  float min_axis = std::min (fx, fy);

  if ((min_axis / max_axis) > axis_ratio_)
  {
    PCL_WARN("Both axis are equally easy/difficult to disambiguate\n");

    Eigen::Vector3f evy_copy = evy;
    Eigen::Vector3f evxminus = evx * -1;
    Eigen::Vector3f evyminus = evy * -1;

    if (min_axis > min_axis_value_)
    {
      //combination of all possibilities
      evy = evx.cross (evz);
      Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      evx = evxminus;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      evx = evy_copy;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      evx = evyminus;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

    }
    else
    {
      //1-st case (evx selected)
      evy = evx.cross (evz);
      Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);

      //2-nd case (evy selected)
      evx = evy_copy;
      evy = evx.cross (evz);
      trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
      transformations.push_back (trans);
    }
  }
  else
  {
    if (fy < fx)
    {
      evx = evy;
      fx = fy;
    }

    evy = evx.cross (evz);
    Eigen::Matrix4f trans = createTransFromAxes (evx, evy, evz, transformPC, center_mat);
    transformations.push_back (trans);

  }

  return true;
}
コード例 #20
0
ファイル: SIM_SnowSolver.c プロジェクト: Azmisov/snow
//Do the interpolation calculations
bool SIM_SnowSolver::solveGasSubclass(SIM_Engine &engine, SIM_Object *obj, SIM_Time time, SIM_Time framerate){

	/// STEP #0: Retrieve all data objects from Houdini

	//Scalar params
	freal particle_mass = getPMass();
	freal YOUNGS_MODULUS = getYoungsModulus();
	freal POISSONS_RATIO = getPoissonsRatio();
	freal CRIT_COMPRESS = getCritComp();
	freal CRIT_STRETCH = getCritStretch();
	freal FLIP_PERCENT = getFlipPercent();
	freal HARDENING = getHardening();
	freal CFL = getCfl();
	freal COF = getCof();
	freal division_size = getDivSize();
	freal max_vel = getMaxVel();
	//Vector params
	vector3 GRAVITY = getGravity();
	vector3 bbox_min_limit = getBboxMin();
	vector3 bbox_max_limit = getBboxMax();

	//Particle params
	UT_String s_p, s_vol, s_den, s_vel, s_fe, s_fp;
	getParticles(s_p);
	getPVol(s_vol);
	getPD(s_den);
	getPVel(s_vel);
	getPFe(s_fe);
	getPFp(s_fp);

	SIM_Geometry* geometry = (SIM_Geometry*) obj->getNamedSubData(s_p);
	if (!geometry) return true;
	
	//Get particle data
	//Do we use the attribute name???
	// GU_DetailHandle gdh = geometry->getGeometry().getWriteableCopy();
	GU_DetailHandle gdh = geometry->getOwnGeometry();
	const GU_Detail* gdp_in = gdh.readLock(); // Must unlock later
	GU_Detail* gdp_out = gdh.writeLock();

	GA_RWAttributeRef p_ref_position = gdp_out->findPointAttribute("P");
	GA_RWHandleT<vector3> p_position(p_ref_position.getAttribute());

	GA_RWAttributeRef p_ref_volume = gdp_out->findPointAttribute(s_vol);
	GA_RWHandleT<freal> p_volume(p_ref_volume.getAttribute());

	GA_RWAttributeRef p_ref_density = gdp_out->findPointAttribute(s_den);
	GA_RWHandleT<freal> p_density(p_ref_density.getAttribute());

	GA_RWAttributeRef p_ref_vel = gdp_out->findPointAttribute(s_vel);
	GA_RWHandleT<vector3> p_vel(p_ref_vel.getAttribute());

	GA_RWAttributeRef p_ref_Fe = gdp_out->findPointAttribute(s_fe);
	GA_RWHandleT<matrix3> p_Fe(p_ref_Fe.getAttribute());

	GA_RWAttributeRef p_ref_Fp = gdp_out->findPointAttribute(s_fp);
	GA_RWHandleT<matrix3> p_Fp(p_ref_Fp.getAttribute());

	//EVALUATE PARAMETERS
	freal mu = YOUNGS_MODULUS/(2+2*POISSONS_RATIO);
	freal lambda = YOUNGS_MODULUS*POISSONS_RATIO/((1+POISSONS_RATIO)*(1-2*POISSONS_RATIO));

	//Get grid data
	SIM_ScalarField *g_mass_field;
	SIM_DataArray g_mass_data;
	getMatchingData(g_mass_data, obj, MPM_G_MASS);	
	g_mass_field = SIM_DATA_CAST(g_mass_data(0), SIM_ScalarField);

	SIM_VectorField *g_nvel_field;
	SIM_DataArray g_nvel_data;
	getMatchingData(g_nvel_data, obj, MPM_G_NVEL);
	g_nvel_field = SIM_DATA_CAST(g_nvel_data(0), SIM_VectorField);

	SIM_VectorField *g_ovel_field;
	SIM_DataArray g_ovel_data;
	getMatchingData(g_ovel_data, obj, MPM_G_OVEL);
	g_ovel_field = SIM_DATA_CAST(g_ovel_data(0), SIM_VectorField);

	SIM_ScalarField *g_active_field;
	SIM_DataArray g_active_data;
	getMatchingData(g_active_data, obj, MPM_G_ACTIVE);	
	g_active_field = SIM_DATA_CAST(g_active_data(0), SIM_ScalarField);

	SIM_ScalarField *g_density_field;
	SIM_DataArray g_density_data;
	getMatchingData(g_density_data, obj, MPM_G_DENSITY);	
	g_density_field = SIM_DATA_CAST(g_density_data(0), SIM_ScalarField);

	SIM_ScalarField *g_col_field;
	SIM_DataArray g_col_data;
	getMatchingData(g_col_data, obj, MPM_G_COL);	
	g_col_field = SIM_DATA_CAST(g_col_data(0), SIM_ScalarField);

	SIM_VectorField *g_colVel_field;
	SIM_DataArray g_colVel_data;
	getMatchingData(g_colVel_data, obj, MPM_G_COLVEL);	
	g_colVel_field = SIM_DATA_CAST(g_colVel_data(0), SIM_VectorField);

	SIM_VectorField *g_extForce_field;
	SIM_DataArray g_extForce_data;
	getMatchingData(g_extForce_data, obj, MPM_G_EXTFORCE);	
	g_extForce_field = SIM_DATA_CAST(g_extForce_data(0), SIM_VectorField);
	
	UT_VoxelArrayF
		*g_mass = g_mass_field->getField()->fieldNC(),
		*g_nvelX = g_nvel_field->getField(0)->fieldNC(),
		*g_nvelY = g_nvel_field->getField(1)->fieldNC(),
		*g_nvelZ = g_nvel_field->getField(2)->fieldNC(),
		*g_ovelX = g_ovel_field->getField(0)->fieldNC(),
		*g_ovelY = g_ovel_field->getField(1)->fieldNC(),
		*g_ovelZ = g_ovel_field->getField(2)->fieldNC(),
		*g_colVelX = g_colVel_field->getField(0)->fieldNC(),
		*g_colVelY = g_colVel_field->getField(1)->fieldNC(),
		*g_colVelZ = g_colVel_field->getField(2)->fieldNC(),
		*g_extForceX = g_extForce_field->getField(0)->fieldNC(),
		*g_extForceY = g_extForce_field->getField(1)->fieldNC(),
		*g_extForceZ = g_extForce_field->getField(2)->fieldNC(),
		*g_col = g_col_field->getField()->fieldNC(),
		*g_active = g_active_field->getField()->fieldNC();

	int point_count = gdp_out->getPointRange().getEntries();
	std::vector<boost::array<freal,64> > p_w(point_count);
	std::vector<boost::array<vector3,64> > p_wgh(point_count);

	//Get world-to-grid conversion ratios
	//Particle's grid position can be found via (pos - grid_origin)/voxel_dims
	vector3
		voxel_dims = g_mass_field->getVoxelSize(),
		grid_origin = g_mass_field->getOrig(),
		grid_divs = g_mass_field->getDivisions();
	//Houdini uses voxel centers for grid nodes, rather than grid corners
	grid_origin += voxel_dims/2.0;
	freal voxelArea = voxel_dims[0]*voxel_dims[1]*voxel_dims[2];
	
	/*
	//Reset grid
	for(int iX=0; iX < grid_divs[0]; iX++){
		for(int iY=0; iY < grid_divs[1]; iY++){
			for(int iZ=0; iZ < grid_divs[2]; iZ++){
				g_mass->setValue(iX,iY,iZ,0);
				g_active->setValue(iX,iY,iZ,0);
				g_ovelX->setValue(iX,iY,iZ,0);
				g_ovelY->setValue(iX,iY,iZ,0);
				g_ovelZ->setValue(iX,iY,iZ,0);
				g_nvelX->setValue(iX,iY,iZ,0);
				g_nvelY->setValue(iX,iY,iZ,0);
				g_nvelZ->setValue(iX,iY,iZ,0);
			}
		}
	}
	*/

	/// STEP #1: Transfer mass to grid

	if (p_position.isValid()){

		//Iterate through particles
		for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){
			int pid = it.getOffset();
							
			//Get grid position
			vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
			int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
			//g_mass_field->posToIndex(p_position.get(pid),p_gridx,p_gridy,p_gridz);
			freal particle_density = p_density.get(pid);
			//Compute weights and transfer mass
			for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
				//Z-dimension interpolation
				freal z_pos = gpos[2]-z,
					wz = SIM_SnowSolver::bspline(z_pos),
					dz = SIM_SnowSolver::bsplineSlope(z_pos);
				for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
					//Y-dimension interpolation
					freal y_pos = gpos[1]-y,
						wy = SIM_SnowSolver::bspline(y_pos),
						dy = SIM_SnowSolver::bsplineSlope(y_pos);
					for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
						//X-dimension interpolation
						freal x_pos = gpos[0]-x,
							wx = SIM_SnowSolver::bspline(x_pos),
							dx = SIM_SnowSolver::bsplineSlope(x_pos);
						
						//Final weight is dyadic product of weights in each dimension
						freal weight = wx*wy*wz;
						p_w[pid-1][idx] = weight;

						//Weight gradient is a vector of partial derivatives
						p_wgh[pid-1][idx] = vector3(dx*wy*wz, wx*dy*wz, wx*wy*dz)/voxel_dims;

						//Interpolate mass
						freal node_mass = g_mass->getValue(x,y,z);
						node_mass += weight*particle_mass;
						g_mass->setValue(x,y,z,node_mass);
					}
				}
			}
		}
	}
	
	/// STEP #2: First timestep only - Estimate particle volumes using grid mass

	/*
	if (time == 0.0){
		//Iterate through particles
		for (GA_Iterator it(gdp_out->getPointRange()); !it.atEnd(); it.advance()){
			int pid = it.getOffset();
			freal density = 0;

			//Get grid position
			int p_gridx = 0, p_gridy = 0, p_gridz = 0;
			vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
			int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
			//g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz);
			//Transfer grid density (within radius) to particles
			for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
				for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
					for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
						freal w = p_w[pid-1][idx];
						if (w > EPSILON){
							//Transfer density
							density += w * g_mass->getValue(x,y,z);
						}
					}
				}
			}
			
			density /= voxelArea;
			p_density.set(pid,density);
			p_volume.set(pid, particle_mass/density);
		}
	}
	//*/
	
	/// STEP #3: Transfer velocity to grid

	//This must happen after transferring mass, to conserve momentum
	//Iterate through particles and transfer
	for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
		int pid = it.getOffset();
		vector3 vel_fac = p_vel.get(pid)*particle_mass;

		//Get grid position
		vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
		int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
		//g_nvel_field->posToIndex(0,p_position.get(pid),p_gridx,p_gridy,p_gridz);

		//Transfer to grid nodes within radius
		for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
			for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
				for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
					freal w = p_w[pid-1][idx];
					if (w > EPSILON){
						freal nodex_vel = g_ovelX->getValue(x,y,z) + vel_fac[0]*w;
						freal nodey_vel = g_ovelY->getValue(x,y,z) + vel_fac[1]*w;
						freal nodez_vel = g_ovelZ->getValue(x,y,z) + vel_fac[2]*w;
						g_ovelX->setValue(x,y,z,nodex_vel);
						g_ovelY->setValue(x,y,z,nodey_vel);
						g_ovelZ->setValue(x,y,z,nodez_vel);			
						g_active->setValue(x,y,z,1.0);			
					}
				}
			}
		}
	}
	//Division is slow (maybe?); we only want to do divide by mass once, for each active node
	for(int iX=0; iX < grid_divs[0]; iX++){
		for(int iY=0; iY < grid_divs[1]; iY++){
			for(int iZ=0; iZ < grid_divs[2]; iZ++){
				//Only check nodes that have mass
				if (g_active->getValue(iX,iY,iZ)){
					freal node_mass = 1/(g_mass->getValue(iX,iY,iZ));
					g_ovelX->setValue(iX,iY,iZ,(g_ovelX->getValue(iX,iY,iZ)*node_mass));
					g_ovelY->setValue(iX,iY,iZ,(g_ovelY->getValue(iX,iY,iZ)*node_mass));
					g_ovelZ->setValue(iX,iY,iZ,(g_ovelZ->getValue(iX,iY,iZ)*node_mass));
				}
			}
		}
	}
	
	/// STEP #4: Compute new grid velocities

	//Temporary variables for plasticity and force calculation
	//We need one set of variables for each thread that will be running
	eigen_matrix3 def_elastic, def_plastic, energy, svd_u, svd_v;
	Eigen::JacobiSVD<eigen_matrix3, Eigen::NoQRPreconditioner> svd;
	eigen_vector3 svd_e;
	matrix3  HDK_def_plastic, HDK_def_elastic, HDK_energy;
	freal* data_dp = HDK_def_plastic.data();
	freal* data_de = HDK_def_elastic.data();
	freal* data_energy = HDK_energy.data();
	//Map Eigen matrices to HDK matrices
	Eigen::Map<eigen_matrix3> data_dp_map(data_dp);
	Eigen::Map<eigen_matrix3> data_de_map(data_de);
	Eigen::Map<eigen_matrix3> data_energy_map(data_energy);	

	//Compute force at each particle and transfer to Eulerian grid
	//We use "nvel" to hold the grid force, since that variable is not in use
	for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
		int pid = it.getOffset();
		
		//Apply plasticity to deformation gradient, before computing forces
		//We need to use the Eigen lib to do the SVD; transfer houdini matrices to Eigen matrices
		HDK_def_plastic = p_Fp.get(pid);
		HDK_def_elastic = p_Fe.get(pid);
		def_plastic = Eigen::Map<eigen_matrix3>(data_dp);
		def_elastic = Eigen::Map<eigen_matrix3>(data_de);
		
		//Compute singular value decomposition (uev*)
		svd.compute(def_elastic, Eigen::ComputeFullV | Eigen::ComputeFullU);
		svd_e = svd.singularValues();
		svd_u = svd.matrixU();
		svd_v = svd.matrixV();
		//Clamp singular values
		for (int i=0; i<3; i++){
			if (svd_e[i] < CRIT_COMPRESS) 
				svd_e[i] = CRIT_COMPRESS;
			else if (svd_e[i] > CRIT_STRETCH)
				svd_e[i] = CRIT_STRETCH;
		}
		//Put SVD back together for new elastic and plastic gradients
		def_plastic = svd_v * svd_e.asDiagonal().inverse() * svd_u.transpose() * def_elastic * def_plastic;
		svd_v.transposeInPlace();
		def_elastic = svd_u * svd_e.asDiagonal() * svd_v;
		
		//Now compute the energy partial derivative (which we use to get force at each grid node)
		energy = 2*mu*(def_elastic - svd_u*svd_v)*def_elastic.transpose();
		//Je is the determinant of def_elastic (equivalent to svd_e.prod())
		freal Je = svd_e.prod(),
			contour = lambda*Je*(Je-1),
			jp = def_plastic.determinant(),
			particle_vol = p_volume.get(pid);
		for (int i=0; i<3; i++)
			energy(i,i) += contour;
		energy *=  particle_vol * exp(HARDENING*(1-jp));
		
		//Transfer Eigen matrices back to HDK
		data_dp_map = def_plastic;
		data_de_map = def_elastic;
		data_energy_map = energy;
		
		p_Fp.set(pid,HDK_def_plastic);
		p_Fe.set(pid,HDK_def_elastic);
		
		//Transfer energy to surrounding grid nodes
		vector3 gpos = (p_position.get(pid) - grid_origin)/voxel_dims;
		int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
		for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
			for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
				for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
					freal w = p_w[pid-1][idx];
					if (w > EPSILON){
						vector3 ngrad = p_wgh[pid-1][idx];
						g_nvelX->setValue(x,y,z,g_nvelX->getValue(x,y,z) + ngrad.dot(HDK_energy[0]));
						g_nvelY->setValue(x,y,z,g_nvelY->getValue(x,y,z) + ngrad.dot(HDK_energy[1]));
						g_nvelZ->setValue(x,y,z,g_nvelZ->getValue(x,y,z) + ngrad.dot(HDK_energy[2]));						
					}
				}
			}
		}
	}

	//Use new forces to solve for new velocities
	for(int iX=0; iX < grid_divs[0]; iX++){
		for(int iY=0; iY < grid_divs[1]; iY++){
			for(int iZ=0; iZ < grid_divs[2]; iZ++){
				//Only compute for active nodes
				if (g_active->getValue(iX,iY,iZ)){
					freal nodex_ovel = g_ovelX->getValue(iX,iY,iZ);
					freal nodey_ovel = g_ovelY->getValue(iX,iY,iZ);
					freal nodez_ovel = g_ovelZ->getValue(iX,iY,iZ);
					freal forcex = g_nvelX->getValue(iX,iY,iZ);
					freal forcey = g_nvelY->getValue(iX,iY,iZ);
					freal forcez = g_nvelZ->getValue(iX,iY,iZ);
					freal node_mass = 1/(g_mass->getValue(iX,iY,iZ));
					freal ext_forceX = GRAVITY[0] + g_extForceX->getValue(iX,iY,iZ);
					freal ext_forceY = GRAVITY[1] + g_extForceY->getValue(iX,iY,iZ);
					freal ext_forceZ = GRAVITY[2] + g_extForceZ->getValue(iX,iY,iZ);
					nodex_ovel += framerate*(ext_forceX - forcex*node_mass);
					nodey_ovel += framerate*(ext_forceY - forcey*node_mass);
					nodez_ovel += framerate*(ext_forceZ - forcez*node_mass);
					
					//Limit velocity to max_vel
					vector3 g_nvel(nodex_ovel, nodey_ovel, nodez_ovel);
					freal nvelNorm = g_nvel.length();
					if(nvelNorm > max_vel){
						freal velRatio = max_vel/nvelNorm;
						g_nvel*= velRatio;
					}

					g_nvelX->setValue(iX,iY,iZ,g_nvel[0]);
					g_nvelY->setValue(iX,iY,iZ,g_nvel[1]);
					g_nvelZ->setValue(iX,iY,iZ,g_nvel[2]);

				}
			}
		}
	}

	/// STEP #5: Grid collision resolution

	vector3 sdf_normal;
	//*
	for(int iX=1; iX < grid_divs[0]-1; iX++){
		for(int iY=1; iY < grid_divs[1]-1; iY++){
			for(int iZ=1; iZ < grid_divs[2]-1; iZ++){
				if (g_active->getValue(iX,iY,iZ)){
					if (!computeSDFNormal(g_col, iX, iY, iZ, sdf_normal))
						continue;

					//Collider velocity
					vector3 vco(
						g_colVelX->getValue(iX,iY,iZ),
						g_colVelY->getValue(iX,iY,iZ),
						g_colVelZ->getValue(iX,iY,iZ)
					);
					//Grid velocity
					vector3 v(
						g_nvelX->getValue(iX,iY,iZ),
						g_nvelY->getValue(iX,iY,iZ),
						g_nvelZ->getValue(iX,iY,iZ)
					);
					//Skip if bodies are separating
					vector3 vrel = v - vco;
					
					freal vn = vrel.dot(sdf_normal);
					if (vn >= 0) continue;
					//Resolve collisions; also add velocity of collision object to snow velocity
					//Sticks to surface (too slow to overcome static friction)
					vector3 vt = vrel - (sdf_normal*vn);

					freal stick = vn*COF, vt_norm = vt.length();
					if (vt_norm <= -stick)
						vt = vco;
					//Dynamic friction
					else vt += stick*vt/vt_norm + vco;
					
					g_nvelX->setValue(iX,iY,iZ,vt[0]);	
					g_nvelY->setValue(iX,iY,iZ,vt[1]);
					g_nvelZ->setValue(iX,iY,iZ,vt[2]);
				}
			}
		}
	}
	//*/

	/// STEP #6: Transfer grid velocities to particles and integrate
	/// STEP #7: Particle collision resolution

	vector3 pic, flip, col_vel;
	matrix3 vel_grad;
	//Iterate through particles
	for (GA_Iterator it(gdp_in->getPointRange()); !it.atEnd(); it.advance()){
		int pid = it.getOffset();
		//Particle position
		vector3 pos(p_position.get(pid));
		
		//Reset velocity
		pic[0] = 0.0;
		pic[1] = 0.0;
		pic[2] = 0.0;
		flip = p_vel.get(pid);
		vel_grad.zero();
		freal density = 0;

		 //Get grid position
		vector3 gpos = (pos - grid_origin)/voxel_dims;
		int p_gridx = (int) gpos[0], p_gridy = (int) gpos[1], p_gridz = (int) gpos[2];
		for (int idx=0, z=p_gridz-1, z_end=z+3; z<=z_end; z++){
			for (int y=p_gridy-1, y_end=y+3; y<=y_end; y++){
				for (int x=p_gridx-1, x_end=x+3; x<=x_end; x++, idx++){
					freal w = p_w[pid-1][idx];
					if (w > EPSILON){
						const vector3 node_wg = p_wgh[pid-1][idx];
						const vector3 node_nvel(
							g_nvelX->getValue(x,y,z),
							g_nvelY->getValue(x,y,z),
							g_nvelZ->getValue(x,y,z)
						);

						//Transfer velocities
						pic += node_nvel*w;	
						flip[0] += (node_nvel[0] - g_ovelX->getValue(x,y,z))*w;	
						flip[1] += (node_nvel[1]- g_ovelY->getValue(x,y,z))*w;	
						flip[2] += (node_nvel[2] - g_ovelZ->getValue(x,y,z))*w;
						//Transfer density
						density += w * g_mass->getValue(x,y,z);
						//Transfer veloctiy gradient
						vel_grad.outerproductUpdate(1.0, node_nvel, node_wg);
					}
				}
			}
		}

		//Finalize velocity update
		vector3 vel = flip*FLIP_PERCENT + pic*(1-FLIP_PERCENT);
		
		//Reset collision data
		freal col_sdf = 0;
		sdf_normal[0] = 0;
		sdf_normal[1] = 0;
		sdf_normal[2] = 0;
		col_vel[0] = 0;
		col_vel[1] = 0;
		col_vel[2] = 0;
		
		//Interpolate surrounding nodes' SDF info to the particle (trilinear interpolation)

		for (int idx=0, z=p_gridz, z_end=z+1; z<=z_end; z++){
			freal w_z = gpos[2]-z;
			for (int y=p_gridy, y_end=y+1; y<=y_end; y++){
				freal w_zy = w_z*(gpos[1]-y);
				for (int x=p_gridx, x_end=x+1; x<=x_end; x++, idx++){
					freal weight = fabs(w_zy*(gpos[0]-x));
					//cout << w_zy << "," << (gpos[0]-x) << "," << weight << endl;
					vector3 temp_normal;
					computeSDFNormal(g_col, x, y, z, temp_normal);
						//goto SKIP_PCOLLIDE;
					//cout << g_col->getValue(x, y, z) << endl;
					//Interpolate
					sdf_normal += temp_normal*weight;
					col_sdf += g_col->getValue(x, y, z)*weight;
					col_vel[0] += g_colVelX->getValue(x, y, z)*weight;
					col_vel[1] += g_colVelY->getValue(x, y, z)*weight;
					col_vel[2] += g_colVelZ->getValue(x, y, z)*weight;
				}
			}
		}

		//Resolve particle collisions	
		//cout << col_sdf << endl;
		if (col_sdf > 0){
			vector3 vrel = vel - col_vel;
			freal vn = vrel.dot(sdf_normal);
			
			//Skip if bodies are separating
			if (vn < 0){
				
				//Resolve and add velocity of collision object to snow velocity
				//Sticks to surface (too slow to overcome static friction)
				vel = vrel - (sdf_normal*vn);
				freal stick = vn*COF, vel_norm = vel.length();
				if (vel_norm <= -stick)
					vel = col_vel;
				//Dynamic friction
				else vel += stick*vel/vel_norm + col_vel;
			}
		}
		
		SKIP_PCOLLIDE:

		//Finalize density update
		density /= voxelArea;
		p_density.set(pid,density);

		//Update particle position
		pos += framerate*vel;
		//Limit particle position
		int mask = 0;
		/*		
		for (int i=0; i<3; i++){
			if (pos[i] > bbox_max_limit[i]){
				pos[i] = bbox_max_limit[i];
				vel[i] = 0.0;
				mask |= 1 << i;
			}
			else if (pos[i] < bbox_min_limit[i]){
				pos[i] = bbox_min_limit[i];
				vel[i] = 0.0;
				mask |= 1 << i;
			}
		}
		//Slow particle down at bounds (not really necessary...)
		if (mask){
			for (int i=0; i<3; i++){
				if (mask & 0x1)
					vel[i] *= .05;
				mask >>= 1;
			}
		}//*/
		p_vel.set(pid,vel);
		p_position.set(pid,pos);

		//Update particle deformation gradient
		//Note: plasticity is computed on the next timestep...
		vel_grad *= framerate;
		vel_grad(0,0) += 1;
		vel_grad(1,1) += 1;
		vel_grad(2,2) += 1;
		
		p_Fe.set(pid, vel_grad*p_Fe.get(pid));
	}

	gdh.unlock(gdp_out);
    gdh.unlock(gdp_in);
	
	return true;
}
コード例 #21
0
Types::Transform PlaneToPlaneCalibration::estimateTransform(const std::vector<PlanePair> & plane_pair_vector)
{
  const int size = plane_pair_vector.size();

  Eigen::MatrixXd normals_1(3, size);
  Eigen::MatrixXd normals_2(3, size);
  Eigen::VectorXd distances_1(size);
  Eigen::VectorXd distances_2(size);

  for (int i = 0; i < size; ++i)
  {
    const Types::Plane &plane_1 = plane_pair_vector[i].plane_1_;
    const Types::Plane &plane_2 = plane_pair_vector[i].plane_2_;

    if (plane_1.offset() > 0)
    {
      normals_1.col(i) = -plane_1.normal();
      distances_1(i) = plane_1.offset();
    }
    else
    {
      normals_1.col(i) = plane_1.normal();
      distances_1(i) = -plane_1.offset();
    }

    if (plane_2.offset() > 0)
    {
      normals_2.col(i) = -plane_2.normal();
      distances_2(i) = plane_2.offset();
    }
    else
    {
      normals_2.col(i) = plane_2.normal();
      distances_2(i) = -plane_2.offset();
    }
  }

//  std::cout << normals_1 << std::endl;
//  std::cout << distances_1.transpose() << std::endl;
//  std::cout << normals_2 << std::endl;
//  std::cout << distances_2.transpose() << std::endl;

  Eigen::Matrix3d USV = normals_2 * normals_1.transpose();
  Eigen::JacobiSVD<Eigen::Matrix3d> svd;
  svd.compute(USV, Eigen::ComputeFullU | Eigen::ComputeFullV);

  Types::Pose pose;
  pose.translation() = (normals_1 * normals_1.transpose()).inverse() * normals_1 * (distances_1 - distances_2);
  pose.linear() = svd.matrixV() * svd.matrixU().transpose();

//  // Point-Plane Constraints
//
//  // Initial system (Eq. 10)
//
//  Eigen::MatrixXd system(size * 3, 13);
//  for (int i = 0; i < size; ++i)
//  {
//    const double d = plane_pair_vector[i].plane_1_.offset();
//    const Eigen::Vector3d n = plane_pair_vector[i].plane_1_.normal();
//
//    const Point3d x = -plane_pair_vector[i].plane_2_.normal() * plane_pair_vector[i].plane_2_.offset();
//
//    Eigen::Matrix3d X(Eigen::Matrix3d::Random());
//    while (X.determinant() < 1e-5)
//      X = Eigen::Matrix3d::Random();
//
//    const Eigen::Vector3d & n2 = plane_pair_vector[i].plane_2_.normal();
////    X.col(1)[2] = -n2.head<2>().dot(X.col(1).head<2>()) / n2[2];
////    X.col(2)[2] = -n2.head<2>().dot(X.col(2).head<2>()) / n2[2];
//    X.col(1) -= n2 * n2.dot(X.col(1));
//    X.col(2) -= n2 * n2.dot(X.col(2));
//
//    X.col(0) = x;
//    X.col(1) += x;
//    X.col(2) += x;
//
//    for (int j = 0; j < 3; ++j)
//    {
//      const Point3d & x = X.col(j);
//
//      system.row(i + size * j) << d + n[0] * x[0] + n[1] * x[1] + n[2] * x[2],  // q_0^2
//                                  2 * n[2] * x[1] - 2 * n[1] * x[2],            // q_0 * q_1
//                                  2 * n[0] * x[2] - 2 * n[2] * x[0],            // q_0 * q_2
//                                  2 * n[1] * x[0] - 2 * n[0] * x[1],            // q_0 * q_3
//                                  d + n[0] * x[0] - n[1] * x[1] - n[2] * x[2],  // q_1^2
//                                  2 * n[0] * x[1] + 2 * n[1] * x[0],            // q_1 * q_2
//                                  2 * n[0] * x[2] + 2 * n[2] * x[0],            // q_1 * q_3
//                                  d - n[0] * x[0] + n[1] * x[1] - n[2] * x[2],  // q_2^2
//                                  2 * n[1] * x[2] + 2 * n[2] * x[1],            // q_2 * q_3
//                                  d - n[0] * x[0] - n[1] * x[1] + n[2] * x[2],  // q_3^2
//                                  n[0], n[1], n[2]; // q'*q*t
//    }
//  }
//
//  //std::cout << system << std::endl;
//
//  // Gaussian reduction
//  for (int k = 0; k < 3; ++k)
//    for (int i = k + 1; i < size * 3; ++i)
//      system.row(i) = system.row(i) - system.row(k) * system.row(i)[10 + k] / system.row(k)[10 + k];
//
//  //std::cout << system << std::endl;
//
//  // Quaternion q
//  Eigen::Vector4d q;
//
//  // Transform to inhomogeneous (Eq. 13)
//  bool P_is_ok(false);
//  while (not P_is_ok)
//  {
//    Eigen::Matrix4d P(Eigen::Matrix4d::Random().normalized());
//    while (P.determinant() < 1e-5)
//      P = Eigen::Matrix4d::Random().normalized();
//
//    Eigen::MatrixXd reduced_system(size * 3 - 3, 10);
//    for (int i = 3; i < size * 3; ++i)
//    {
//      const Eigen::VectorXd & row = system.row(i);
//      Eigen::Matrix4d Mi_tilde;
//
//      Mi_tilde << row[0]    , row[1] / 2, row[2] / 2, row[3] / 2,
//                  row[1] / 2, row[4]    , row[5] / 2, row[6] / 2,
//                  row[2] / 2, row[5] / 2, row[7]    , row[8] / 2,
//                  row[3] / 2, row[6] / 2, row[8] / 2, row[9]    ;
//
//      Eigen::Matrix4d Mi_bar(P.transpose() * Mi_tilde * P);
//
//      reduced_system.row(i - 3) << Mi_bar(0, 0),
//                                   Mi_bar(0, 1) + Mi_bar(1, 0),
//                                   Mi_bar(0, 2) + Mi_bar(2, 0),
//                                   Mi_bar(0, 3) + Mi_bar(3, 0),
//                                   Mi_bar(1, 1),
//                                   Mi_bar(1, 2) + Mi_bar(2, 1),
//                                   Mi_bar(1, 3) + Mi_bar(3, 1),
//                                   Mi_bar(2, 2),
//                                   Mi_bar(2, 3) + Mi_bar(3, 2),
//                                   Mi_bar(3, 3);
//    }
//
//    // Solve  A m* = b
//    Eigen::MatrixXd A = reduced_system.rightCols<9>();
//
//    Eigen::VectorXd b = - reduced_system.leftCols<1>();
//    Eigen::VectorXd m_star = A.jacobiSvd(Eigen::ComputeFullU | Eigen::ComputeFullV).solve(b);
//
//    Eigen::Vector4d q_bar(1, m_star[0], m_star[1], m_star[2]);
//
//    Eigen::VectorXd err(6);
//    err << q_bar[1] * q_bar[1],
//           q_bar[1] * q_bar[2],
//           q_bar[1] * q_bar[3],
//           q_bar[2] * q_bar[2],
//           q_bar[2] * q_bar[3],
//           q_bar[3] * q_bar[3];
//    err -= m_star.tail<6>();
//
//    if (err.norm() < 0.1) // P is ok?
//      P_is_ok = true;
//
//    q = P * q_bar;
//  }
//
//  // We want q.w > 0 (Why?)
//  if (q[0] < 0)
//    q = -q;
//  Eigen::Quaterniond rotation(q[0], q[1], q[2], q[3]);
//  rotation.normalize();
//
//  Eigen::VectorXd m(10);
//  m << q[0] * q[0],
//       q[0] * q[1],
//       q[0] * q[2],
//       q[0] * q[3],
//       q[1] * q[1],
//       q[1] * q[2],
//       q[1] * q[3],
//       q[2] * q[2],
//       q[2] * q[3],
//       q[3] * q[3];
//
//  // Solve A (q^T q t) = b
//
//  Eigen::Matrix3d A = system.topRightCorner<3, 3>();
//  Eigen::Vector3d b = - system.topLeftCorner<3, 10>() * m;
//
//  std::cout << A << " " << b.transpose() << std::endl;
//  Eigen::Translation3d translation(A.colPivHouseholderQr().solve(b) / q.squaredNorm());
//
//  Eigen::Quaterniond tmp(pose.linear());
//  Eigen::Translation3d tmp2(pose.translation());
//  std::cout << "Prev: " << tmp.normalized().coeffs().transpose() << " " << tmp2.vector().transpose() << std::endl;
//  std::cout << "New : " << rotation.coeffs().transpose() << " " << translation.vector().transpose() << std::endl;
//
//  pose = translation * rotation;

  return pose;
}