本文整理汇总了C++中eigen::JacobiSVD类的典型用法代码示例。如果您正苦于以下问题:C++ JacobiSVD类的具体用法?C++ JacobiSVD怎么用?C++ JacobiSVD使用的例子?那么恭喜您, 这里精选的类代码示例或许可以为您提供帮助。
在下文中一共展示了JacobiSVD类的20个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。
示例1: svd
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> ¢roid_src,
const Eigen::Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &cloud_tgt_demean,
const Eigen::Matrix<Scalar, 4, 1> ¢roid_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;
}
开发者ID:5irius,项目名称:pcl,代码行数:32,代码来源:transformation_estimation_svd.hpp
示例2: EstimateTfSVD
// 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();
}
开发者ID:mruan,项目名称:range_calib,代码行数:32,代码来源:linearTF_solver.hpp
示例3: pose_estimation_3d3d
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 ) );
}
开发者ID:gaoxiang12,项目名称:slambook,代码行数:57,代码来源:pose_estimation_3d3d.cpp
示例4: svd
template <typename PointSource, typename PointTarget, typename Scalar> void
pcl::registration::TransformationEstimationSVDScale<PointSource, PointTarget, Scalar>::getTransformationFromCorrelation (
const Eigen::MatrixXf &cloud_src_demean,
const Eigen::Vector4f ¢roid_src,
const Eigen::MatrixXf &cloud_tgt_demean,
const Eigen::Vector4f ¢roid_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;
}
开发者ID:kalectro,项目名称:pcl_groovy,代码行数:54,代码来源:transformation_estimation_svd_scale.hpp
示例5: evaluateSVDSolver
void evaluateSVDSolver(const Eigen::MatrixXd& A, const Eigen::VectorXd& b,
const Eigen::VectorXd& x) {
// const double before = aslam::calibration::Timestamp::now();
const Eigen::JacobiSVD<Eigen::MatrixXd> svd(A,
Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::VectorXd x_est = svd.solve(b);
// const double after = aslam::calibration::Timestamp::now();
// const double error = (b - A * x_est).norm();
// std::cout << std::fixed << std::setprecision(18) << "error: " << error
// << " est_diff: " << (x - x_est).norm() << " time: " << after - before
// << std::endl;
// std::cout << "estimated rank: " << svd.nonzeroSingularValues() << std::endl;
// std::cout << "estimated rank deficiency: "
// << A.cols() - svd.nonzeroSingularValues() << std::endl;
}
开发者ID:AliAlawieh,项目名称:kalibr,代码行数:15,代码来源:LinearSolverTest.cpp
示例6: ctms_decompositions
void ctms_decompositions()
{
const int maxSize = 16;
const int size = 12;
typedef Eigen::Matrix<Scalar,
Eigen::Dynamic, Eigen::Dynamic,
0,
maxSize, maxSize> Matrix;
typedef Eigen::Matrix<Scalar,
Eigen::Dynamic, 1,
0,
maxSize, 1> Vector;
typedef Eigen::Matrix<std::complex<Scalar>,
Eigen::Dynamic, Eigen::Dynamic,
0,
maxSize, maxSize> ComplexMatrix;
const Matrix A(Matrix::Random(size, size));
const ComplexMatrix complexA(ComplexMatrix::Random(size, size));
const Matrix saA = A.adjoint() * A;
// Cholesky module
Eigen::LLT<Matrix> LLT; LLT.compute(A);
Eigen::LDLT<Matrix> LDLT; LDLT.compute(A);
// Eigenvalues module
Eigen::HessenbergDecomposition<ComplexMatrix> hessDecomp; hessDecomp.compute(complexA);
Eigen::ComplexSchur<ComplexMatrix> cSchur(size); cSchur.compute(complexA);
Eigen::ComplexEigenSolver<ComplexMatrix> cEigSolver; cEigSolver.compute(complexA);
Eigen::EigenSolver<Matrix> eigSolver; eigSolver.compute(A);
Eigen::SelfAdjointEigenSolver<Matrix> saEigSolver(size); saEigSolver.compute(saA);
Eigen::Tridiagonalization<Matrix> tridiag; tridiag.compute(saA);
// LU module
Eigen::PartialPivLU<Matrix> ppLU; ppLU.compute(A);
Eigen::FullPivLU<Matrix> fpLU; fpLU.compute(A);
// QR module
Eigen::HouseholderQR<Matrix> hQR; hQR.compute(A);
Eigen::ColPivHouseholderQR<Matrix> cpQR; cpQR.compute(A);
Eigen::FullPivHouseholderQR<Matrix> fpQR; fpQR.compute(A);
// SVD module
Eigen::JacobiSVD<Matrix> jSVD; jSVD.compute(A, ComputeFullU | ComputeFullV);
}
开发者ID:Aerobota,项目名称:c2tam,代码行数:48,代码来源:nomalloc.cpp
示例7: demeanPointCloud
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;
}
开发者ID:diegodgs,项目名称:PCL,代码行数:46,代码来源:sac_model_registration.hpp
示例8: main
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;
}
开发者ID:Asuzer,项目名称:cgal,代码行数:46,代码来源:optimal_rotation_svd_eigen.cpp
示例9: estimateRigidTransformationSVD
/**
* 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.;
}
开发者ID:ToMadoRe,项目名称:v4r,代码行数:49,代码来源:RigidTransformationRANSAC.cpp
示例10: MatrixXr_pseudoInverse
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;
}
开发者ID:CrazyHeex,项目名称:woo,代码行数:28,代码来源:Math.cpp
示例11: matSVD
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);
}
开发者ID:schdomin,项目名称:vi_mapper,代码行数:18,代码来源:CMiniVisionToolbox.cpp
示例12: A
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT>::planeFitting (
Eigen::Matrix<float,
Eigen::Dynamic, 3> const &points,
Eigen::Vector3f ¢er,
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);
}
开发者ID:2php,项目名称:pcl,代码行数:39,代码来源:board.hpp
示例13: svd
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);
}
开发者ID:Bastl34,项目名称:PCL,代码行数:23,代码来源:transformation_from_correspondences.hpp
示例14: pinv
// 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;
}
开发者ID:GT-RAIL,项目名称:carl_moveit,代码行数:45,代码来源:eigen_pinv.hpp
示例15: pseudoInverse
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();
}
开发者ID:mikewiltero,项目名称:Sub8,代码行数:20,代码来源:cv_tools.hpp
示例16: pseudo_inverse_svd
// 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;
}
开发者ID:zhanglx13,项目名称:Hybrid_Computation_System,代码行数:68,代码来源:PseudoInverseSVD.hpp
示例17: min_quad_dense_precompute
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;
}
开发者ID:bbrrck,项目名称:libigl,代码行数:77,代码来源:min_quad_dense.cpp
示例18: pow
template<typename PointSource, typename PointTarget> void
pcl::NormalDistributionsTransform<PointSource, PointTarget>::computeTransformation (PointCloudSource &output, const Eigen::Matrix4f &guess)
{
nr_iterations_ = 0;
converged_ = false;
double gauss_c1, gauss_c2, gauss_d3;
// Initializes the guassian fitting parameters (eq. 6.8) [Magnusson 2009]
gauss_c1 = 10 * (1 - outlier_ratio_);
gauss_c2 = outlier_ratio_ / pow (resolution_, 3);
gauss_d3 = -log (gauss_c2);
gauss_d1_ = -log ( gauss_c1 + gauss_c2 ) - gauss_d3;
gauss_d2_ = -2 * log ((-log ( gauss_c1 * exp ( -0.5 ) + gauss_c2 ) - gauss_d3) / gauss_d1_);
if (guess != Eigen::Matrix4f::Identity ())
{
// Initialise final transformation to the guessed one
final_transformation_ = guess;
// Apply guessed transformation prior to search for neighbours
transformPointCloud (output, output, guess);
}
// Initialize Point Gradient and Hessian
point_gradient_.setZero ();
point_gradient_.block<3, 3>(0, 0).setIdentity ();
point_hessian_.setZero ();
Eigen::Transform<float, 3, Eigen::Affine, Eigen::ColMajor> eig_transformation;
eig_transformation.matrix () = final_transformation_;
// Convert initial guess matrix to 6 element transformation vector
Eigen::Matrix<double, 6, 1> p, delta_p, score_gradient;
Eigen::Vector3f init_translation = eig_transformation.translation ();
Eigen::Vector3f init_rotation = eig_transformation.rotation ().eulerAngles (0, 1, 2);
p << init_translation (0), init_translation (1), init_translation (2),
init_rotation (0), init_rotation (1), init_rotation (2);
Eigen::Matrix<double, 6, 6> hessian;
double score = 0;
double delta_p_norm;
// Calculate derivates of initial transform vector, subsequent derivative calculations are done in the step length determination.
score = computeDerivatives (score_gradient, hessian, output, p);
while (!converged_)
{
// Store previous transformation
previous_transformation_ = transformation_;
// Solve for decent direction using newton method, line 23 in Algorithm 2 [Magnusson 2009]
Eigen::JacobiSVD<Eigen::Matrix<double, 6, 6> > sv (hessian, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Negative for maximization as opposed to minimization
delta_p = sv.solve (-score_gradient);
//Calculate step length with guarnteed sufficient decrease [More, Thuente 1994]
delta_p_norm = delta_p.norm ();
if (delta_p_norm == 0 || delta_p_norm != delta_p_norm)
{
trans_probability_ = score / static_cast<double> (input_->points.size ());
converged_ = delta_p_norm == delta_p_norm;
return;
}
delta_p.normalize ();
delta_p_norm = computeStepLengthMT (p, delta_p, delta_p_norm, step_size_, transformation_epsilon_ / 2, score, score_gradient, hessian, output);
delta_p *= delta_p_norm;
transformation_ = (Eigen::Translation<float, 3> (static_cast<float> (delta_p (0)), static_cast<float> (delta_p (1)), static_cast<float> (delta_p (2))) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (3)), Eigen::Vector3f::UnitX ()) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (4)), Eigen::Vector3f::UnitY ()) *
Eigen::AngleAxis<float> (static_cast<float> (delta_p (5)), Eigen::Vector3f::UnitZ ())).matrix ();
p = p + delta_p;
// Update Visualizer (untested)
if (update_visualizer_ != 0)
update_visualizer_ (output, std::vector<int>(), *target_, std::vector<int>() );
if (nr_iterations_ > max_iterations_ ||
(nr_iterations_ && (std::fabs (delta_p_norm) < transformation_epsilon_)))
{
converged_ = true;
}
nr_iterations_++;
}
// Store transformation probability. The realtive differences within each scan registration are accurate
// but the normalization constants need to be modified for it to be globally accurate
trans_probability_ = score / static_cast<double> (input_->points.size ());
}
开发者ID:kalectro,项目名称:pcl_groovy,代码行数:97,代码来源:ndt.hpp
示例19: estimateTransform
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];
//.........这里部分代码省略.........
开发者ID:iaslab-unipd,项目名称:calibration_toolkit,代码行数:101,代码来源:plane_to_plane_calibration.cpp
示例20: transformPC
template<typename PointInT, typename PointNT, typename PointOutT> bool
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT
|
请发表评论