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

    typename boost::math::tools::promote_args<T_y, T_shape>::type
    lkj_corr_log(const Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
                 const T_shape& eta) {
      static const char* function("lkj_corr_log");

      using boost::math::tools::promote_args;

      typename promote_args<T_y, T_shape>::type lp(0.0);
      check_positive(function, "Shape parameter", eta);
      check_corr_matrix(function, "Correlation matrix", y);

      const unsigned int K = y.rows();
      if (K == 0)
        return 0.0;

      if (include_summand<propto, T_shape>::value)
        lp += do_lkj_constant(eta, K);

      if ( (eta == 1.0) &&
           stan::is_constant<typename stan::scalar_type<T_shape> >::value )
        return lp;

      if (!include_summand<propto, T_y, T_shape>::value)
        return lp;

      Eigen::Matrix<T_y, Eigen::Dynamic, 1> values =
        y.ldlt().vectorD().array().log().matrix();
      lp += (eta - 1.0) * sum(values);
      return lp;
    }

void ConstraintManager::computeConstraintForces()
{
	for (std::vector< FixedDistanceConstraint* >::iterator iter = m_constraintForces.begin(); iter < m_constraintForces.end(); ++iter)
	{
		(*iter)->populateConstraintMatrix( m_constraint );
		(*iter)->populateJacobian( m_jacobian );
		(*iter)->populateJacobianDerivative( m_jacobianDeriv );
	}
	Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > invMassMatrix;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > velocityCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > forceCurrent;
	m_particleManager.getInvMassMatrix( invMassMatrix );
	m_particleManager.getCurrentState( velocityCurrent, false, true );
	m_particleManager.getCurrentDerivative( forceCurrent, false, true );
	m_constraintDeriv = m_jacobian * velocityCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > jacobianInvJacobianT = m_jacobian * invMassMatrix * m_jacobian.transpose();
	Eigen::Matrix< double, Eigen::Dynamic, 1 > jacobianDerivVelocity = m_jacobianDeriv * velocityCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > jacobianInvMassForce = m_jacobian * invMassMatrix * forceCurrent;
	Eigen::Matrix< double, Eigen::Dynamic, 1 > b = -jacobianDerivVelocity - jacobianInvMassForce - m_ks*m_constraint - m_kd*m_constraintDeriv;
	if (jacobianInvJacobianT.determinant() != 0)
	{
		m_lagrangeMultipliers = jacobianInvJacobianT.ldlt().solve( b );
	}
	else
	{
		m_lagrangeMultipliers.resize( m_constraintForces.size() );
		m_lagrangeMultipliers.setZero();
	}
}

 inline bool check_pos_definite(const char* function,
                                const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y,
                                const char* name,
                                T_result* result) {
   typedef 
     typename Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>::size_type 
     size_type;
   if (y.rows() == 1 && y(0,0) <= CONSTRAINT_TOLERANCE) {
     std::ostringstream message;
     message << name << " is not positive definite. " 
             << name << "(0,0) is %1%.";
     std::string msg(message.str());
     return dom_err(function,y(0,0),name,msg.c_str(),"",result);
   }
   Eigen::LDLT< Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic> > cholesky 
     = y.ldlt();
   if(cholesky.info() != Eigen::Success || 
      cholesky.isNegative() ||
      (cholesky.vectorD().array() <= CONSTRAINT_TOLERANCE).any()) {
     std::ostringstream message;
     message << name << " is not positive definite. " 
             << name << "(0,0) is %1%.";
     std::string msg(message.str());
     return dom_err(function,y(0,0),name,msg.c_str(),"",result);
   }
   return true;
 }

Eigen::Matrix<double, N, N> cholesky(Eigen::Matrix<double, N, N> x) {
  Eigen::LDLT<Eigen::Matrix<double, N, N> > ldlt = x.ldlt();
  Eigen::Array<double, N, 1> d(ldlt.vectorD().array());
  for(int i = 0; i < N; i++) {
    if(d[i] < 0) d[i] = 0;
  }
  Eigen::Matrix<double, N, 1> sqrtd(d.sqrt());
  return ldlt.transpositionsP().transpose() * Eigen::Matrix<double, N, N>(ldlt.matrixL()) * sqrtd.asDiagonal();
}

    inline T log_determinant_spd(const Eigen::Matrix<T,R,C>& m) {
      using std::log;
      stan::error_handling::check_square("log_determinant_spd", "m", m);
//      Eigen::TriangularView< Eigen::Matrix<T,R,C>, Eigen::Lower > L(m.llt().matrixL());
//      T ret(0.0);
//      for (size_t i = 0; i < L.rows(); i++)
//        ret += log(L(i,i));
//      return 2*ret;
      return m.ldlt().vectorD().array().log().sum();
    }

    inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
    inv_wishart_rng(const double nu,
                    const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic>& S,
                    RNG& rng) {

      Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> S_inv(S.rows(), S.cols());
      S_inv = Eigen::MatrixXd::Identity(S.cols(),S.cols());
      S_inv = S.ldlt().solve(S_inv);

      return wishart_rng(nu, S_inv, rng).inverse();
    }

    inline Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic>
    inv_wishart_rng(const double nu,
                    const Eigen::Matrix
                    <double, Eigen::Dynamic, Eigen::Dynamic>& S,
                    RNG& rng) {
      static const char* function("stan::math::inv_wishart_rng");

      using stan::math::check_greater;
      using stan::math::check_square;
      using Eigen::MatrixXd;
      using stan::math::index_type;

      typename index_type<MatrixXd>::type k = S.rows();

      check_greater(function, "Degrees of freedom parameter", nu, k-1);
      check_square(function, "scale parameter", S);

      MatrixXd S_inv = MatrixXd::Identity(k, k);
      S_inv = S.ldlt().solve(S_inv);

      return wishart_rng(nu, S_inv, rng).inverse();
    }

void Optimizer::optimizeUseG2O()
{


    // create the linear solver
    BlockSolverX::LinearSolverType * linearSolver = new LinearSolverCSparse<BlockSolverX::PoseMatrixType>();

    // create the block solver on top of the linear solver
    BlockSolverX* blockSolver = new BlockSolverX(linearSolver);

    // create the algorithm to carry out the optimization
    //OptimizationAlgorithmGaussNewton* optimizationAlgorithm = new OptimizationAlgorithmGaussNewton(blockSolver);
    OptimizationAlgorithmLevenberg* optimizationAlgorithm = new OptimizationAlgorithmLevenberg(blockSolver);

    // NOTE: We skip to fix a variable here, either this is stored in the file
    // itself or Levenberg will handle it.

    // create the optimizer to load the data and carry out the optimization
    SparseOptimizer optimizer;
    SparseOptimizer::initMultiThreading();
    optimizer.setVerbose(true);
    optimizer.setAlgorithm(optimizationAlgorithm);

    {
        pcl::ScopeTime time("G2O setup Graph vertices");
        for (size_t cloud_count = 0; cloud_count < m_pointClouds.size(); ++cloud_count)
        {
            VertexSE3 *vertex = new VertexSE3;
            vertex->setId(cloud_count);
            Isometry3D affine = Isometry3D::Identity();
            affine.linear() = m_pointClouds[cloud_count]->sensor_orientation_.toRotationMatrix().cast<Isometry3D::Scalar>();
            affine.translation() = m_pointClouds[cloud_count]->sensor_origin_.block<3, 1>(0, 0).cast<Isometry3D::Scalar>();
            vertex->setEstimate(affine);
            optimizer.addVertex(vertex);
        }
        optimizer.vertex(0)->setFixed(true);
    }

    {
        pcl::ScopeTime time("G2O setup Graph edges");
        double trans_noise = 0.5, rot_noise = 0.5235;
        EdgeSE3::InformationType infomation = EdgeSE3::InformationType::Zero();
        infomation.block<3, 3>(0, 0) << trans_noise * trans_noise, 0, 0,
                                        0, trans_noise * trans_noise, 0,
                                        0, 0, trans_noise * trans_noise;
        infomation.block<3, 3>(3, 3) << rot_noise * rot_noise, 0, 0,
                                        0, rot_noise * rot_noise, 0,
                                        0, 0, rot_noise * rot_noise;
        for (size_t pair_count = 0; pair_count < m_cloudPairs.size(); ++pair_count)
        {
            CloudPair pair = m_cloudPairs[pair_count];
		    int from = pair.corresIdx.first;
		    int to = pair.corresIdx.second;
            EdgeSE3 *edge = new EdgeSE3;
		    edge->vertices()[0] = optimizer.vertex(from);
		    edge->vertices()[1] = optimizer.vertex(to);

            Eigen::Matrix<double, 6, 6> ATA = Eigen::Matrix<double, 6, 6>::Zero();
            Eigen::Matrix<double, 6, 1> ATb = Eigen::Matrix<double, 6, 1>::Zero();
#pragma unroll 8
            for (size_t point_count = 0; point_count < pair.corresPointIdx.size(); ++point_count) {
                int point_p = pair.corresPointIdx[point_count].first;
                int point_q = pair.corresPointIdx[point_count].second;
                PointType P = m_pointClouds[from]->points[point_p];
                PointType Q = m_pointClouds[to]->points[point_q];

                Eigen::Vector3d p = P.getVector3fMap().cast<double>();
                Eigen::Vector3d q = Q.getVector3fMap().cast<double>();
                Eigen::Vector3d Np = P.getNormalVector3fMap().cast<double>();

                double b = (p - q).dot(Np);

                Eigen::Matrix<double, 6, 1> A_p;
                A_p.block<3, 1>(0, 0) = p.cross(Np);
                A_p.block<3, 1>(3, 0) = Np;

                ATA += A_p * A_p.transpose();
                ATb += A_p * b;
            }

            Eigen::Matrix<double, 6, 1> X = ATA.ldlt().solve(ATb);
            Isometry3D measure = Isometry3D::Identity();
            float beta = X[0];
            float gammar = X[1];
            float alpha = X[2];
            measure.linear() = (Eigen::Matrix3d)Eigen::AngleAxisd(alpha, Eigen::Vector3d::UnitZ()) *
                Eigen::AngleAxisd(gammar, Eigen::Vector3d::UnitY()) *
                Eigen::AngleAxisd(beta, Eigen::Vector3d::UnitX());
            measure.translation() = X.block<3, 1>(3, 0);

            edge->setMeasurement(measure);

		    edge->setInformation(infomation);
            
            optimizer.addEdge(edge);
        }
    }

    optimizer.save("debug_preOpt.g2o");
    {
//.........这里部分代码省略.........

//.........这里部分代码省略.........
                    nextImage[pyramidLevel],
                    imageBasis,
                    kinv,
                    krlr,
                    sumDataSO3,
                    outDataSO3,
                    jtj.data(),
                    jtr.data(),
                    &residual[0],
                    GPUConfig::getInstance().so3StepThreads,
                    GPUConfig::getInstance().so3StepBlocks);
            TOCK("so3Step");

            lastSO3Error = sqrt(residual[0]) / residual[1];
            lastSO3Count = residual[1];

            //Converged
            if(lastSO3Error < lastError && lastCount == lastSO3Count)
            {
                break;
            }
            else if(lastSO3Error > lastError + 0.001) //Diverging
            {
                lastSO3Error = lastError;
                lastSO3Count = lastCount;
                resultR = lastResultR;
                break;
            }

            lastError = lastSO3Error;
            lastCount = lastSO3Count;
            lastResultR = resultR;

            Eigen::Vector3f delta = jtj.ldlt().solve(jtr);

            Eigen::Matrix<double, 3, 3, Eigen::RowMajor> rotUpdate = OdometryProvider::rodrigues(delta.cast<double>());

            R_lr = rotUpdate.cast<float>() * R_lr;

            for(int x = 0; x < 3; x++)
            {
                for(int y = 0; y < 3; y++)
                {
                    resultR(x, y) = R_lr(x, y);
                }
            }
        }
    }

    iterations[0] = fastOdom ? 3 : 10;
    iterations[1] = pyramid ? 5 : 0;
    iterations[2] = pyramid ? 4 : 0;

    Eigen::Matrix<float, 3, 3, Eigen::RowMajor> Rprev_inv = Rprev.inverse();
    mat33 device_Rprev_inv = Rprev_inv;
    float3 device_tprev = *reinterpret_cast<float3*>(tprev.data());

    Eigen::Matrix<double, 4, 4, Eigen::RowMajor> resultRt = Eigen::Matrix<double, 4, 4, Eigen::RowMajor>::Identity();

    if(so3)
    {
        for(int x = 0; x < 3; x++)
        {
            for(int y = 0; y < 3; y++)
            {
                resultRt(x, y) = resultR(x, y);

        bool CameraModel::_lift( const Eigen::MatrixBase<Derived1>& mP2D,
                                 Eigen::MatrixBase<Derived2>& mP3D ) const {
        typedef typename Eigen::internal::traits<Derived2>::Scalar ScalarType;
        const unsigned int nNumPoints = mP2D.cols();
#if 0
        typedef Eigen::Matrix<ScalarType,1,Eigen::Dynamic> RowVect;
        const int nWidth = 1000;
        const int nHeight = 1000;
        const int nWidthSize = nWidth/10;
        const int nHeightSize = nHeight/10;
        Derived1 mP2DUndist( 3, nWidthSize*nHeightSize );
        Derived1 m1 = RowVect::LinSpaced( nWidthSize, 0, nWidth-1 ).replicate( nHeightSize, 1 );
        mP2DUndist.row( 0 ) = Eigen::Map<Derived1>( m1.data(), 1, nWidthSize*nHeightSize );
        mP2DUndist.row( 1 ) = RowVect::LinSpaced( nHeightSize, 0, nHeight-1 ).replicate( 1, nWidthSize );
        mP2DUndist.row( 2 ) = RowVect::Ones( 1, nWidthSize*nHeightSize );
        Derived1 mP2DDist( 2, nWidthSize*nHeightSize );
        project( mP2DUndist, mP2DDist );

        // Initialise with closest values
        for( int ii=0; ii<mP2D.cols(); ii++ ) {
            //Derived1::Index nMinIndex;
            int nMinIndex;
            Derived1 mDiff = mP2DDist;
            mDiff.colwise() -= mP2D.col( ii );
            mDiff.colwise().norm().minCoeff( &nMinIndex );
            mP3D.col( ii ) = mP2DUndist.col( nMinIndex );
        }
#else
        // Start from distortionless points if possible
        if( get( "fx" ) != "" && get( "fy" ) != "" && 
            get( "cx" ) != "" && get( "cy" ) != "" ) {
            double fx = get<double>( "fx" ); double fy = get<double>( "fy" );
            double cx = get<double>( "cx" ); double cy = get<double>( "cy" );
            mP3D.row( 0 ) = ( mP2D.row( 0 ) - cx*Derived2::Ones( 1, nNumPoints ) ) / fx;
            mP3D.row( 1 ) = ( mP2D.row( 1 ) - cy*Derived2::Ones( 1, nNumPoints ) ) / fy;
            mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );            
        }
        else {
            mP3D.row( 0 ) = mP2D.row( 0 );
            mP3D.row( 1 ) = mP2D.row( 1 );
            mP3D.row( 2 ) = Derived2::Ones( 1, nNumPoints );
        }
#endif
        for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
            mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
        }

        Derived1 mP2DEst( 2, nNumPoints );
        Derived2 mdP2DE( 2, 3*nNumPoints );
        Derived2 mdP2DI( 2, get_num_parameters()*nNumPoints );

        double dMaxErr = 0.001;
        double dLastMaxErr = 10;
        const unsigned int nMaxIter = 30;
        const unsigned int nMaxOuterIter = 5;
        for( size_t nOuterIter=0; nOuterIter<nMaxOuterIter; nOuterIter++ ) {
            for( size_t nIter=0; nIter<nMaxIter; nIter++ ) {
                project( mP3D, mP2DEst, mdP2DE, mdP2DI );
                for( size_t nIndex=0; nIndex<nNumPoints; nIndex++ ) {
                    Eigen::Matrix<ScalarType,2,3> mJ = mdP2DE.block( 0, 3*nIndex, 2, 3 );
                    Eigen::Matrix<ScalarType,3,3> mJtJ = mJ.transpose()*mJ + 0.1*Eigen::Matrix<ScalarType,3,3>::Identity();
                    Eigen::Matrix<ScalarType,3,1> mJte = mJ.transpose() * ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) );
                    mP3D.col( nIndex ) -= mJtJ.ldlt().solve( mJte );
                
                    double dErr = ( mP2DEst.col( nIndex ) - mP2D.col( nIndex ) ).norm();
                    if( nIndex > 0 ) {
                        if( std::isnan( dErr ) ) {
                            mP3D.col( nIndex ) =  mP3D.col( nIndex-1 );
                        }
                    }
                    mP3D.col( nIndex ) /= mP3D.col( nIndex ).norm();
                }
                dLastMaxErr = ( mP2DEst - mP2D ).colwise().norm().maxCoeff();
                if( dLastMaxErr < dMaxErr ) {
                    break;
                }
            }

            if( dLastMaxErr >= dMaxErr ) {
                Derived1 mErrors = ( mP2DEst - mP2D ).colwise().norm();
                for( int ii=0; ii<mErrors.cols(); ii++ ) {
                    if( mErrors(0,ii) > dMaxErr ) {
                        // Find closest value
                        double dMinDiff = 1e10;
                        int nBestIndex = -1;
                        for( int jj=0; jj<mErrors.cols(); jj++ ) {
                            if( jj != ii && mErrors(0,jj) < dMaxErr ) {
                                double dDiff = ( mP2D.col( ii ) - mP2D.col( jj ) ).norm();
                                if( dDiff <  dMinDiff ) { 
                                    dMinDiff = dDiff;
                                    nBestIndex = jj;
                                }
                            }
                        }
                        if( nBestIndex == -1 ) {
                            // All is lost, could not find an index 
                            // that passes the error test
                            return false;
                        }
                        mP3D.col( ii ) = mP3D.col( nBestIndex ) ;
//.........这里部分代码省略.........