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

void populateTestPoseGraph(PoseGraph & graph)
{
    // Build a sample pose graph.
    //   2 |                       --> 12 --> 13 --> 14 <-- 15 --> 16
    //     |                      /                   A
    //   1 |                  /-> 6 --> 7 -->  8       \-----
    //     |                 /                 V            |
    // Y 0 |     1 --> 2 <-- 3              -> 9 --> 10 --> 11
    //     |                 \             /  
    //  -1 |                  \-> 4 --> 5 /          17 --> 18 --> 19
    //     |
    //     ------------------------------------------------------------
    //          0     1     2    3     4      5     6      7        8
    //                               X

    // Add the vertices.
    for(int i = 1; i <= 19; i++)
    {
        graph.addVertex(VertexId(i));
    }

    Eigen::Matrix<double,6,1> udiag;
    udiag << 0.1 ,0.1 ,0.1,    // Translation
        0.01,0.01,0.01;   // Rotation
    Eigen::Matrix<double,6,6> U = udiag.asDiagonal();

    // Add the edges: addEdge(to,from,T_to_from
    graph.addEdge (VertexId(2), VertexId(1),   UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*1));
    graph.addEdge (VertexId(2), VertexId(3),   UncertainTransformation(toTEuler( 1, 0, 0, 0, 0, 0), U*2));
    graph.addEdge (VertexId(4), VertexId(3),   UncertainTransformation(toTEuler(-1, 1, 0, 0, 0, 0), U*3));
    graph.addEdge (VertexId(6), VertexId(3),   UncertainTransformation(toTEuler(-1,-1, 0, 0, 0, 0), U*4));
    graph.addEdge (VertexId(5), VertexId(4),   UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*5));
    graph.addEdge (VertexId(7), VertexId(6),   UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*6));
    graph.addEdge (VertexId(8), VertexId(7),   UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*7));
    graph.addEdge (VertexId(9), VertexId(8),   UncertainTransformation(toTEuler( 0, 1, 0, 0, 0, 0), U*8));
    graph.addEdge (VertexId(9), VertexId(5),   UncertainTransformation(toTEuler(-1,-1, 0, 0, 0, 0), U*9));
    graph.addEdge (VertexId(10), VertexId(9),  UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*10));  
    graph.addEdge (VertexId(11), VertexId(10), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*11));
    graph.addEdge (VertexId(14), VertexId(11), UncertainTransformation(toTEuler( 1,-2, 0, 0, 0, 0), U*12));
    graph.addEdge (VertexId(12), VertexId(6),  UncertainTransformation(toTEuler(-1,-1, 0, 0, 0, 0), U*13));
    graph.addEdge (VertexId(13), VertexId(12), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*14));
    graph.addEdge (VertexId(14), VertexId(13), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*15));
    graph.addEdge (VertexId(14), VertexId(15), UncertainTransformation(toTEuler( 1, 0, 0, 0, 0, 0), U*16));
    graph.addEdge (VertexId(16), VertexId(15), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*17));
    graph.addEdge (VertexId(18), VertexId(17), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*18));
    graph.addEdge (VertexId(19), VertexId(18), UncertainTransformation(toTEuler(-1, 0, 0, 0, 0, 0), U*19));



    // This works.

}

    typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
    multi_gp_log(const Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic>& y,
                 const Eigen::Matrix
                 <T_covar, Eigen::Dynamic, Eigen::Dynamic>& Sigma,
                 const Eigen::Matrix<T_w, Eigen::Dynamic, 1>& w) {
      static const char* function("multi_gp_log");
      typedef typename boost::math::tools::promote_args<T_y, T_covar, T_w>::type
        T_lp;
      T_lp lp(0.0);


      check_positive(function, "Kernel rows", Sigma.rows());
      check_finite(function, "Kernel", Sigma);
      check_symmetric(function, "Kernel", Sigma);

      LDLT_factor<T_covar, Eigen::Dynamic, Eigen::Dynamic> ldlt_Sigma(Sigma);
      check_ldlt_factor(function, "LDLT_Factor of Sigma", ldlt_Sigma);

      check_size_match(function,
                       "Size of random variable (rows y)", y.rows(),
                       "Size of kernel scales (w)", w.size());
      check_size_match(function,
                       "Size of random variable", y.cols(),
                       "rows of covariance parameter", Sigma.rows());
      check_positive_finite(function, "Kernel scales", w);
      check_finite(function, "Random variable", y);

      if (y.rows() == 0)
        return lp;

      if (include_summand<propto>::value) {
        lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
      }

      if (include_summand<propto, T_covar>::value) {
        lp -= 0.5 * log_determinant_ldlt(ldlt_Sigma) * y.rows();
      }

      if (include_summand<propto, T_w>::value) {
        lp += (0.5 * y.cols()) * sum(log(w));
      }

      if (include_summand<propto, T_y, T_w, T_covar>::value) {
        Eigen::Matrix<T_w, Eigen::Dynamic, Eigen::Dynamic>
          w_mat(w.asDiagonal());
        Eigen::Matrix<T_y, Eigen::Dynamic, Eigen::Dynamic> yT(y.transpose());
        lp -= 0.5 * trace_gen_inv_quad_form_ldlt(w_mat, ldlt_Sigma, yT);
      }

      return lp;
    }

   ErrorTermTransformation::ErrorTermTransformation(aslam::backend::TransformationExpression T, sm::kinematics::Transformation prior, double weightRotation, double weightTranslation) :
 _T(T), _prior(prior), _debug(0)
   {
       Eigen::Matrix<double, 6, 1> W;
       W << weightTranslation, weightTranslation, weightTranslation, weightRotation, weightRotation, weightRotation;
       
       // Fill in the inverse covariance.
       setInvR(Eigen::Matrix<double,6,6>(W.asDiagonal()));
       
       // Tell the super class about the design variables:
       aslam::backend::DesignVariable::set_t dv;
       _T.getDesignVariables(dv);
       setDesignVariablesIterator(dv.begin(), dv.end());
   }

void createRotationProblems
(
	size_t nr_problems,
	float max_angle,
	float max_compression,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>& F,
	Vcl::Core::InterleavedArray<float, 3, 3, -1>* R
)
{
	// Random number generator
	std::mt19937_64 rng;
	std::uniform_real_distribution<float> d;
	std::uniform_real_distribution<float> a{ -max_angle, max_angle };
	
	for (int i = 0; i < (int)nr_problems; i++)
	{
		// Rest-state
		Eigen::Matrix3f X0;
		X0 << d(rng), d(rng), d(rng),
		      d(rng), d(rng), d(rng),
		      d(rng), d(rng), d(rng);

		// Rotation angle
		float angle = a(rng);

		// Rotation axis
		Eigen::Matrix<float, 3, 1> rot_vec;
		rot_vec << d(rng), d(rng), d(rng);
		rot_vec.normalize();

		// Rotation matrix
		Eigen::Matrix3f Rot = Eigen::AngleAxis<float>{ angle, rot_vec }.toRotationMatrix();
		if (R)
			R->template at<float>(i) = Rot;

		if (max_compression > 0)
		{
			Eigen::Matrix<float, 3, 1> scaling;
			scaling << (1.0f - max_compression*d(rng)), (1.0f - max_compression*d(rng)), (1.0f - max_compression*d(rng));

			Eigen::JacobiSVD<Eigen::Matrix3f> svd{ Rot, Eigen::ComputeFullU | Eigen::ComputeFullV };
			Rot *= svd.matrixV() * scaling.asDiagonal() * svd.matrixV().transpose();
		}

		Eigen::Matrix3f X = Rot * X0;
		F.at<float>(i) = X * X0.inverse();
	}
}

    typename boost::math::tools::promote_args<T_y,T_w,T_covar>::type
    multi_gp_log(const Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic>& y,
                 const Eigen::Matrix<T_covar,Eigen::Dynamic,Eigen::Dynamic>& Sigma,
                 const Eigen::Matrix<T_w,Eigen::Dynamic,1>& w) {
      static const char* function = "stan::prob::multi_gp_log(%1%)";
      typedef typename boost::math::tools::promote_args<T_y,T_w,T_covar>::type T_lp;
      T_lp lp(0.0);
      
      using stan::math::log;
      using stan::math::sum;
      using stan::math::check_not_nan;
      using stan::math::check_size_match;
      using stan::math::check_positive;
      using stan::math::check_finite;
      using stan::math::check_symmetric;
      using stan::math::dot_product;
      using stan::math::rows_dot_product;
      using stan::math::log_determinant_ldlt;
      using stan::math::mdivide_right_ldlt;
      using stan::math::trace_gen_inv_quad_form_ldlt;
      using stan::math::LDLT_factor;
      using stan::math::check_ldlt_factor;

      if (!check_size_match(function, 
                            Sigma.rows(), "Rows of kernel matrix",
                            Sigma.cols(), "columns of kernel matrix",
                            &lp))
        return lp;
      if (!check_positive(function, Sigma.rows(), "Kernel rows", &lp))
        return lp;
      if (!check_finite(function, Sigma, "Kernel", &lp)) 
        return lp;
      if (!check_symmetric(function, Sigma, "Kernel", &lp))
        return lp;
      
      LDLT_factor<T_covar,Eigen::Dynamic,Eigen::Dynamic> ldlt_Sigma(Sigma);
      if(!check_ldlt_factor(function,ldlt_Sigma,"LDLT_Factor of Sigma",&lp))
        return lp;

      if (!check_size_match(function, 
                            y.rows(), "Size of random variable (rows y)",
                            w.size(), "Size of kernel scales (w)",
                            &lp))
        return lp;
      if (!check_size_match(function, 
                            y.cols(), "Size of random variable",
                            Sigma.rows(), "rows of covariance parameter",
                            &lp))
        return lp;
      if (!check_finite(function, w, "Kernel scales", &lp)) 
        return lp;
      if (!check_positive(function, w, "Kernel scales", &lp)) 
        return lp;
      if (!check_finite(function, y, "Random variable", &lp)) 
        return lp;
      
      if (y.rows() == 0)
        return lp;
      
      if (include_summand<propto>::value) {
        lp += NEG_LOG_SQRT_TWO_PI * y.rows() * y.cols();
      }

      if (include_summand<propto,T_covar>::value) {
        lp -= (0.5 * y.rows()) * log_determinant_ldlt(ldlt_Sigma);
      }

      if (include_summand<propto,T_w>::value) {
        lp += (0.5 * y.cols()) * sum(log(w));
      }
      
      if (include_summand<propto,T_y,T_w,T_covar>::value) {
        Eigen::Matrix<T_w,Eigen::Dynamic,Eigen::Dynamic> w_mat(w.asDiagonal());
        Eigen::Matrix<T_y,Eigen::Dynamic,Eigen::Dynamic> yT(y.transpose());
        lp -= 0.5 * trace_gen_inv_quad_form_ldlt(w_mat,ldlt_Sigma,yT);
      }

      return lp;
    }

void
EstimatorFull::SetCovarianceDiagonal(const Eigen::Matrix<double,28,1> &P_)
{
	P = P_.asDiagonal();
}