C++ VectorXd::transpose方法代码示例

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;

}

   bool CartTrajPlanner::jspace_trivial_path_planner(Eigen::VectorXd q_start,Eigen::VectorXd q_end,std::vector<Eigen::VectorXd> &optimal_path) {
       Eigen::VectorXd qx_start(NJNTS),qx_end(NJNTS);// need to convert to this type

       cout<<"jspace_trivial_path_planner: "<<endl;
       cout<<"q_start: "<<q_start.transpose()<<endl;
       cout<<"q_end: "<<q_end.transpose()<<endl;
       optimal_path.clear();
       optimal_path.push_back(q_start);
       optimal_path.push_back(q_end);
       return true;
   }

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);
}

bool ArmMotionInterface::plan_path_to_predefined_pre_pose(void) {
    cout << "called plan_path_to_predefined_pre_pose" << endl;
    cout << "setting q_start and q_end: " << endl;
    q_vec_end_resp_ = q_pre_pose_;
    cout << "q pre-pose goal: " << q_vec_end_resp_.transpose() << endl;
    q_vec_start_resp_ = q_vec_right_arm_;
    cout << "q start = current arm state: " << q_vec_start_resp_.transpose() << endl;
    //plan a path: from q-start to q-goal...what to do with wrist??
    cout << "NOT DONE YET" << endl;
    path_is_valid_ = false;
    return path_is_valid_;

}

bool SufficientZChange::initialize(Eigen::VectorXd& HP) {

	if (_zHistory.size() >= 2) {

	  auto it = _zHistory.begin();
	  const ObservationDescriptor *first = &(it->second);

		while (++it != _zHistory.end()) {
		_zChange = max(_zChange, (it->second.z - first->z).norm());
		}
	}

	if (_zChange > _minZChange && _zHistory.size() >= 5) {
		const ObservationDescriptor &first = _zHistory.begin()->second;

		Eigen::Vector3d z0;
		z0 << first.z(0), first.z(1), 1.0;

		HP << _K.inverse() * z0;
		HP(2) = 1.0 / _initialDepth; // 1/d distance of the plane parallel to the image plane on which features are initialized.

#		ifdef DEBUG_PRINT_VISION_INFO_MESSAGES
		cerr << fixed << "[SufficientZChange] HP initialization: " << HP.transpose() << endl;
#		endif

		return true;
	}

	return false;
}

/* ********************************************************************************************* */
bool getArmConfFull (Part* part, size_t faceId, const Eigen::VectorXd& tL, const Eigen::VectorXd& c, 
		const Eigen::VectorXd& qb, bool right, Eigen::VectorXd& qa) {

	// Compute the location of the point
	Eigen::Matrix4d Tl; 
	createTransform(tL, Tl);
	cout << "c: " << c.transpose() << endl;

	// Get the normal of the face
	// Eigen::Vector3d normal2 = Tl.topLeftCorner<3,3>() * part->getPlane(faceId).topLeftCorner<3,1>();
	// Eigen::Vector3d normal = part->getVertexW(faceId, 0) - part->getVertexW(faceId, 1);
	Eigen::Vector3d normal = part->getPlane(faceId).topLeftCorner<3,1>();
	normal = (Tl.topLeftCorner<3,3>() * normal).normalized();
	Eigen::Matrix3d rotM;
	rotM.block<3,1>(0,2) = -normal;
 	Eigen::Vector3d col2 = Eigen::Vector3d::Random(3);
	rotM.block<3,1>(0,1) = (col2 - col2.dot(normal) * normal).normalized();
	// rotM.block<3,1>(0,1) = normal2;
	rotM.block<3,1>(0,0) = rotM.block<3,1>(0,1).cross(rotM.block<3,1>(0,2));

	// Create the goal matrix
	Matrix4d Twee = Matrix4d::Identity();
	Twee.topRightCorner<3,1>() = c;// - normal * 0.05;
	Twee.topLeftCorner<3,3>() = rotM;

	// Make the call
	Vector7d bla;
	bool result = singleArmIK(qb, Twee, right, bla, true);
	qa = Vector7d(bla);
	if(!result) cout << "can not reach: " << qa(0) << endl;
	return result;
}

    Eigen::VectorXd operator()(const F& obj,  Eigen::VectorXd x) const {
        Eigen::VectorXd g, d;
        Eigen::MatrixXd G;
        LineSearch lineSearch;
        obj(x, g);
        obj(x, G);

        DEBUG_PRINT(x);
        DEBUG_PRINT(g);
        DEBUG_PRINT(G);
        DEBUG_PRINT(G.ldlt().solve(g));
        while(g.norm() > 10e-10) {
#if DEBUG
            std::cout << std::string(20, '-') << std::endl;
#endif
            d = G.ldlt().solve(-g);
            DEBUG_PRINT(G.ldlt().solve(-g).transpose());
            DEBUG_PRINT(G.ldlt().vectorD());

            Real a = 1.;
            a = lineSearch(obj, x, d, a);

            DEBUG_PRINT(a);
            DEBUG_PRINT(obj(x));
            x = x +  a * d;
            obj(x, g);
            obj(x, G);
            DEBUG_PRINT(x.transpose());
            DEBUG_PRINT(g.transpose());
            //	DEBUG_PRINT(G);
        }
        return x;
    }

void Model::construct_preference_and_covariance(
    Eigen::VectorXd &Eta, Eigen::MatrixXd &Omega, const Eigen::MatrixXd &C,
    const Eigen::MatrixXd &M, const Eigen::VectorXd &W,
    const Eigen::MatrixXd &S, const Parameter &parameter) {

  Eigen::VectorXd Mu = C * M * W;
  Eta = Mu;

  Eigen::MatrixXd Psi = W.asDiagonal();
  Psi -= (W * W.transpose());

  Eigen::MatrixXd I(n_alternatives, n_alternatives);
  I.setIdentity();
  Eigen::MatrixXd Phi = C * M * Psi * M.transpose() * C.transpose() +
                        parameter.sig2 * C * I * C.transpose();

  Omega = Phi;
  double rt;
  Eigen::MatrixXd Si = I;

  for (int i = 2; i <= parameter.stopping_time; i++) {
    rt = 1.0 / (1 + exp(i - 202.0) / 25); // [Takao] Not sure what this is.
    Si = S * Si;
    Eta += rt * Si * Mu;
    Omega += pow(rt, 2) * Si * Phi * Si.transpose();
  }
}

void actionStateCallback(const std_msgs::String::ConstPtr& msg)
{
	ROS_WARN("I heard action: [%s]. Doing SHIFT...", msg->data.c_str());
	if (msg->data.c_str()=="reach"){
		shift_ee_ft = est_ee_ft;
		ROS_WARN("Shiftiing!!");
		ROS_WARN_STREAM("Shift: "<<shift_ee_ft.transpose());
	} 
}

        /** Compute the potential of a node according to the weights and its
         * features.
         * \param features: features of the node.
         * \return potentials of that node.
         */
        Eigen::VectorXd computePotentials( Eigen::VectorXd &features )
        {
            // Check that features are a column vector, and transpose it  otherwise
            if ( features.cols() > 1 )
                        features = features.transpose();

            assert ( features.rows() == m_nFeatures );

            return (*m_computePotentialsFunction)(m_weights,features);
        }

void CLMBlockModel::optimise()
{
    this->run(); //Full recompute of all the 0-states, which will never be recomputed again.
	CLMFullModel fullModelFn(this);
    CLevMar LM(fullModelFn, true, 1e-7);
    Eigen::VectorXd params = fullModelFn.init();
    cout << params.transpose() << endl;
    LM.minimise(params, 5);
    cout << "Optimisation complete" << endl;
}

 Eigen::MatrixXd grad(const Eigen::VectorXd& x, const GP& gp) const
 {
     Eigen::MatrixXd grad = Eigen::MatrixXd::Zero(_tr.rows(), _h_params.size());
     Eigen::VectorXd m = _mean_function(x, gp);
     for (int i = 0; i < _tr.rows(); i++) {
         grad.block(i, i * _tr.cols(), 1, _tr.cols() - 1) = m.transpose();
         grad(i, (i + 1) * _tr.cols() - 1) = 1;
     }
     return grad;
 }

int main(int argc, char **argv) {
    ros::init(argc, argv, "rrbot_fk_test");
    ros::NodeHandle nh;

    Eigen::Affine3d affine_flange;

    Rrbot_fwd_solver rrbot_fwd_solver;
    //Rrbot_IK_solver rrbot_ik_solver;    
    
    Eigen::Vector3d flange_origin_wrt_world, perturbed_flange_origin, dp, Jdq_trans;
    Eigen::VectorXd Jdq;

    double q_elbow, q_shoulder; 
    double q_elbow_perturbed,q_shoulder_perturbed;
    double delta_q = 0.000001;
    Eigen::MatrixXd J;
    double diff;
    Eigen::MatrixXd q_vec(2,1),dq_vec(2,1);
    q_vec<<0,0;
    dq_vec<<delta_q,delta_q;

    while (ros::ok()) {
        cout<<endl<<endl;

        q_vec(0) = q_shoulder;
        q_vec(1) = q_elbow;
        ROS_INFO("angs: %f, %f", q_elbow, q_shoulder);

        affine_flange = rrbot_fwd_solver.fwd_kin_flange_wrt_world_solve(q_vec);
        flange_origin_wrt_world = affine_flange.translation();
        cout << "FK: flange origin: " << flange_origin_wrt_world.transpose() << endl;
        //perturb the joint angles and recompute fwd kin:
        affine_flange = rrbot_fwd_solver.fwd_kin_flange_wrt_world_solve(q_vec+dq_vec);
        perturbed_flange_origin = affine_flange.translation();
        dp = perturbed_flange_origin - flange_origin_wrt_world;
        cout<<"dp = "<<dp.transpose()<<endl;
        J = rrbot_fwd_solver.Jacobian(q_vec);
        cout<<"J: "<<endl;
        cout<<J<<endl;
        cout<<"dq = "<<dq_vec.transpose()<<endl;
        Jdq = J*dq_vec;
        cout<<"Jdq = "<<Jdq.transpose()<<endl;
        
        for (int i=0;i<3;i++) Jdq_trans(i) = Jdq(i);

        cout<<"Jdq_trans = "<<Jdq_trans.transpose()<<endl;
        diff = (Jdq_trans - dp).norm();
        cout<<"diff = "<<diff<<endl;
        
        ros::spinOnce();
        ros::Duration(1.0).sleep();
        q_elbow = q_lower_limits[0] + (q_upper_limits[0]-q_lower_limits[0])*(rand() % 100)/100.0;  
        q_shoulder = q_lower_limits[0] + (q_upper_limits[1]-q_lower_limits[1])*(rand() % 100)/100.0;        
    }
}

    void CovarianceEstimator::update(uint i, const Eigen::VectorXd &sample)
    {
      // 10 is theoretically optimal
      double n = (double)lengths_[i] + 10 * sample.size();

      a_[i] = a_[i] * (n / (n + 1)) + (sample * sample.transpose()) / (n + 1);
      u_[i] = u_[i] * (n / (n + 1)) + sample / (n + 1);

      covs_[i] = a_[i] - (u_[i] * u_[i].transpose());// / (n + 1);

      lengths_[i]++;
    }

  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));

  }