C++ MatrixXd::determinant方法代码示例

bool KinematicsMetrics::getManipulabilityIndex(const robot_state::RobotState &state,
                                               const robot_model::JointModelGroup *joint_model_group,
                                               double &manipulability_index,
                                               bool translation) const
{
  // state.getJacobian() only works for chain groups.
  if(!joint_model_group->isChain())
  {
    return false;
  }

  Eigen::MatrixXd jacobian = state.getJacobian(joint_model_group);
  // Get joint limits penalty
  double penalty = getJointLimitsPenalty(state, joint_model_group);
  if (translation)
  {
    Eigen::MatrixXd jacobian_2 = jacobian.topLeftCorner(3,jacobian.cols());
    Eigen::MatrixXd matrix = jacobian_2*jacobian_2.transpose();
    // Get manipulability index
    manipulability_index = penalty * sqrt(matrix.determinant());
  }
  else
  {
    Eigen::MatrixXd matrix = jacobian*jacobian.transpose();
    // Get manipulability index
    manipulability_index = penalty * sqrt(matrix.determinant());
  }
  return true;
}

double weight_gaussian_predictive_rev(Gaussian &g1, Gaussian &g2)
{
    int dim = g1.dim;
    double energy1 = 0.;
            Eigen::MatrixXd cov = g1.predictive_covariance + g1.predictive_covariance;
            Eigen::VectorXd mean = g1.predictive_mean - g1.predictive_mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy1 += gauss;

    double energy2 = 0.;

            cov = g1.predictive_covariance + g2.predictive_covariance;
            mean = g1.predictive_mean - g2.predictive_mean;
            invij = cov.inverse();
            a = mean.transpose()*invij*mean;
            gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy2 += gauss;

    double energy3 = 0.;
            cov = g2.predictive_covariance + g2.predictive_covariance;
            mean = g2.predictive_mean - g2.predictive_mean;
            invij = cov.inverse();
            a = mean.transpose()*invij*mean;
            gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy3 += gauss;
//    cout<<(maxDist - (energy1 - 2*energy2 + energy3))/maxDist<<endl;
    return energy1 - 2*energy2 + energy3;

}

double weight_l2_rev(PCObject &o1, PCObject &o2)
{
    double last = pcl::getTime ();
    // reference :
    // Robust Point Set Registration Using Gaussian Mixture Models
    // Bing Jina, and Baba C. Vemuri
    // IEEE Transactions on Pattern Analysis and Machine Intelligence 2010
    int n = o1.gmm.size();
    int m = o2.gmm.size();

    double energy1 = 0.;
    for(int i=0;i<n;i++){
        for(int j=0;j<n;j++){
            int dim = o1.gmm.at(i).dim;
            Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o1.gmm.at(j).covariance;
            Eigen::VectorXd mean = o1.gmm.at(i).mean - o1.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy1 += o1.gmm.at(i).weight*o1.gmm.at(j).weight*gauss;
        }
    }
    double energy2 = 0.;
    for(int i=0;i<n;i++){
        for(int j=0;j<m;j++){
            int dim = o1.gmm.at(i).dim;
            Eigen::MatrixXd cov = o1.gmm.at(i).covariance + o2.gmm.at(j).covariance;
            Eigen::VectorXd mean = o1.gmm.at(i).mean - o2.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy2 += o1.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
        }
    }
    double energy3 = 0.;
    for(int i=0;i<m;i++){
        for(int j=0;j<m;j++){
            int dim = o2.gmm.at(i).dim;
            Eigen::MatrixXd cov = o2.gmm.at(i).covariance + o2.gmm.at(j).covariance;
            Eigen::VectorXd mean = o2.gmm.at(i).mean - o2.gmm.at(j).mean;
            Eigen::MatrixXd invij = cov.inverse();
            double a = mean.transpose()*invij*mean;
            double gauss = 1./sqrt(pow(2*pi,dim)*cov.determinant())*exp(-0.5*a);
            energy3 += o2.gmm.at(i).weight*o2.gmm.at(j).weight*gauss;
        }
    }
    double now = pcl::getTime ();
//    cout << "l2-distance time " << now-last << " second" << endl;
    //    cout<<"l2distance"<<energy1 - 2*energy2 + energy3<<endl;

    return (energy1 - 2*energy2 + energy3);
}

void mrpt::vision::pnp::rpnp::calcampose(Eigen::MatrixXd& XXc, Eigen::MatrixXd& XXw, Eigen::Matrix3d& R2, Eigen::Vector3d& t2)
{
	Eigen::MatrixXd X = XXc;
	Eigen::MatrixXd Y = XXw;
	Eigen::MatrixXd K = Eigen::MatrixXd::Identity(n, n) - Eigen::MatrixXd::Ones(n, n) * 1 / n;
	Eigen::VectorXd ux, uy;
	uy = X.rowwise().mean();
	ux = Y.rowwise().mean();

	// Need to verify sigmax2
	double sigmax2 = (((X*K).array() * (X*K).array()).colwise().sum()).mean();

	Eigen::MatrixXd SXY = Y*K*(X.transpose()) / n;

	Eigen::JacobiSVD<Eigen::MatrixXd> svd(SXY, Eigen::ComputeThinU | Eigen::ComputeThinV);

	Eigen::Matrix3d S = Eigen::MatrixXd::Identity(3, 3);
	if (SXY.determinant() <0)
		S(2, 2) = -1;

	R2 = svd.matrixV()*S*svd.matrixU().transpose();

	double c2 = (svd.singularValues().asDiagonal()*S).trace() / sigmax2;
	t2 = uy - c2*R2*ux;
	 
	Eigen::Vector3d x, y, z;
	x = R2.col(0);
	y = R2.col(1);
	z = R2.col(2);

	if ((x.cross(y) - z).norm()>0.02)
		R2.col(2) = -R2.col(2);
}

int main() {
        int N                   =       10;
        Eigen::MatrixXd A       =       Eigen::MatrixXd::Random(N,N);
        double detc             =       get_Log_Determinant(A);
        double det              =       log(fabs(A.determinant()));
        std::cout << fabs(detc-det) << std::endl;
}

// computes the optimal rigid body transformation given a set of points
void PoseTracker::computeOptimalRigidTransformation(Eigen::MatrixXd startP, Eigen::MatrixXd finalP)
{	
	Eigen::Matrix4d transf;
	
	if (startP.rows()!=finalP.rows())
	{	
		ROS_ERROR("We need that the rows be the same at the beggining");
		exit(1);
	}

	Eigen::RowVector3d centroid_startP = Eigen::RowVector3d::Zero(); 
	Eigen::RowVector3d centroid_finalP = Eigen::RowVector3d::Zero(); //= mean(B);
	Eigen::Matrix3d H = Eigen::Matrix3d::Zero();

	//calculate the mean
	for (int i=0;i<startP.rows();i++)
	{	
		centroid_startP = centroid_startP+startP.row(i);
		centroid_finalP = centroid_finalP+finalP.row(i);
	}
	
	centroid_startP = centroid_startP/startP.rows();
	centroid_finalP = centroid_finalP/startP.rows();

	for (int i=0;i<startP.rows();i++)
		H = H + (startP.row(i)-centroid_startP).transpose()*(finalP.row(i)-centroid_finalP);

   	Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
   
    Eigen::MatrixXd U = svd.matrixU();
   	Eigen::MatrixXd V = svd.matrixV();
  
    if (V.determinant()<0)
   		V.col(2)=-V.col(2)*(-1);

	Eigen::MatrixXd R = V*U.transpose();

	Eigen::Matrix4d C_A = Eigen::Matrix4d::Identity();
	Eigen::Matrix4d C_B = Eigen::Matrix4d::Identity();
	Eigen::Matrix4d R_new = Eigen::Matrix4d::Identity();
			
	C_A.block<3,1>(0,3) = -centroid_startP.transpose();
	R_new.block<3,3>(0,0) = R;
	
	C_B.block<3,1>(0,3) = centroid_finalP.transpose();


	transf = C_B * R_new * C_A;

	Eigen::Quaterniond mat_rot(transf.block<3,3>(0,0));

	Eigen::Vector3d trasl = transf.block<3,1>(0,3).transpose();

	transfParameters_ << trasl(0), trasl(1), trasl(2), mat_rot.x(), mat_rot.y(), mat_rot.z(), mat_rot.w();

}

double Jacobian::getManipulabilityMeasure(const Eigen::VectorXd& joint_values)
{
  ROS_ASSERT(initialized_);

  Eigen::MatrixXd jac;

  getJacobian(joint_values, jac);

  Eigen::MatrixXd JJT = jac * jac.transpose();
  return sqrt(JJT.determinant());

}

  double multivariateGaussianDensity(const Eigen::VectorXd& mean,
                                         const Eigen::MatrixXd& cov,
                                         const Eigen::VectorXd& z)
  {
    Eigen::VectorXd diff = mean - z;

    Eigen::VectorXd exponent = -0.5 * (diff.transpose() * cov.inverse() * diff);

    return pow(2 * M_PI, (double) z.size() / -2.0) * pow(cov.determinant(), -0.5) *
    exp(exponent(0));

  }

double weight_l2_gauss(PCObject &o1, PCObject &o2)
{
    // l2 distance
    Eigen::MatrixXd covsum = o1.gaussian.covariance+o2.gaussian.covariance;
    Eigen::VectorXd meandiff = o1.gaussian.mean - o2.gaussian.mean;
    Eigen::MatrixXd inv = covsum.inverse();
    double det = covsum.determinant();
    double a = meandiff.transpose()*inv*meandiff;
    double l2 = 2.-2.*(1./sqrt(pow(2*pi,3)*det)) * exp(-0.5*a);
    if(l2 < 0) l2 = 0.;
    return l2;
}

Eigen::Matrix3d RMFE::getRotationMatrix() {
	// Paper: Robust Manhattan Frame Estimation from a Single RGB-D Image
	double lambda = 0.3;
	Eigen::MatrixXd N = this->getNormalMatrix();
	Eigen::Matrix3d R = Eigen::Matrix<double, 3, 3>::Identity();

	float last_error = 10000.0f, error_diff = 10000.0f;
	while (error_diff > 0.0001 || error_diff < 0) {
		Eigen::MatrixXd X = R * N;
		RMFE::applySoftThresholding(X, lambda);
		Eigen::MatrixXd XNTranspose = X * N.transpose();
		Eigen::JacobiSVD<Eigen::MatrixXd> svd(XNTranspose,
				Eigen::ComputeFullU | Eigen::ComputeFullV);
		Eigen::MatrixXd U = svd.matrixU();
		Eigen::MatrixXd V = svd.matrixV();
		Eigen::VectorXd D = svd.singularValues();

		/*std::cout << "U: " << U << std::endl;
		 std::cout << "V: " << V << std::endl;
		 std::cout << "D: " << D << std::endl;*/

		//std::cout << "Remaining: " << (U * D.asDiagonal() * V.transpose() - XNTranspose) << std::endl;
		Eigen::Matrix3d S = Eigen::Matrix<double, 3, 3>::Identity();
		if (XNTranspose.determinant() < 0) {
			S(2, 2) = -1;
		}

		R = U * S * V.transpose();

		float L1NormSum = 0;
		for (int j = 0; j < X.rows(); j++) {
			L1NormSum += X.row(j).lpNorm<1>();
		}
		float error = 0.5 * std::pow((R * N - X).lpNorm<2>(), 2)
				+ lambda * L1NormSum;

		error_diff = last_error - error;
		last_error = error;

		/*std::cout << "Iteration: " << iter << std::endl;
		 std::cout << "R: " << R << std::endl;
		 //std::cout << "X: " << X << std::endl;
		 std::cout << "Error: " << error << std::endl;*/
	}

	return R;
}

int main(int argc, char** argv)
{
    int sequence_number = 1;
    if (argc >= 2)
        sequence_number = atoi(argv[1]);

    ros::init(argc, argv, "figures");
    ROS_INFO("figures");
    ros::NodeHandle n;


    const double timestep = 0.05;
    const double prediction_timestep = 0.05;
    const double sensor_error = 0.005;
    const double collision_probability = 0.95;
    const int acceleration_inference_window_size = 5;
    const int prediction_frames = 6;

    ros::Rate rate(1.0 / timestep);



    KinectPredictor predictor(sequence_number);

    predictor.setTimestep(timestep);
    predictor.setSensorDiagonalCovariance(sensor_error * sensor_error);   // variance is proportional to square of sensing error
    predictor.setCollisionProbability(collision_probability);
    predictor.setAccelerationInferenceWindowSize(acceleration_inference_window_size);

    predictor.setMaximumIterations(5);
    predictor.setGradientDescentMaximumIterations(5);
    predictor.setGradientDescentAlpha(0.005);
    predictor.setHumanShapeLengthConstraintEpsilon(0.01);

    predictor.translate(Eigen::Vector3d(0, 0, -0.1));
    predictor.setVisualizerTopic("figures");

    int frame = 0;
    while (true)
    {
        predictor.moveToNextFrame();

        frame++;
        if (frame == 10)
        {
            FILE* fp = fopen("../data/figures/circles.txt", "w");

            // to visualize
            predictor.visualizePointcloud();
            predictor.visualizeHuman();
            for (int future_frame_index = 0; future_frame_index < prediction_frames; future_frame_index++)
                predictor.visualizePrediction(future_frame_index * prediction_timestep);

            // to retrieve gaussian distribution
            std::vector<Eigen::Vector3d> mu;
            std::vector<Eigen::Matrix3d> sigma;
            std::vector<double> radius;

            predictor.getPredictedGaussianDistribution(0, mu, sigma, radius);

            for (int j=0; j<mu.size(); j++)
            {
                // to YZ plane
                const double y = mu[j](1);
                const double z = mu[j](2);
                const double r = radius[j];

                const Eigen::Matrix2d sigma2 = sigma[j].block(1, 1, 2, 2);
                Eigen::JacobiSVD<Eigen::MatrixXd> svd(sigma2, Eigen::ComputeThinU | Eigen::ComputeThinV);
                Eigen::MatrixXd U = svd.matrixU();
                if (U.determinant() < 0.)
                    U.col(1) *= -1.;

                /*
                fprintf(fp, "%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
                        y, z, r,
                        U(0,0), U(1,0), svd.singularValues()(0),
                        U(0,1), U(1,1), svd.singularValues()(1));
                        */

                fprintf(fp, "%lf,%lf,%lf,%lf,%lf,%lf,%lf\n",
                        y, z, r,
                        sigma2(0,0), sigma2(1,0), sigma2(0,1), sigma2(1,1));
            }

            fclose(fp);

            const Pointcloud& pointcloud = predictor.pointcloud();
            fp = fopen("../data/figures/pointcloud.txt", "w");
            for (int i=0; i<pointcloud.size(); i++)
            {
                const Eigen::Vector3d& v = pointcloud.point(i);
                fprintf(fp, "%lf,%lf\n", v(1), v(2));
            }
            fclose(fp);

            break;
        }
    }

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