本文整理汇总了C++中eigen::VectorXd::transpose方法的典型用法代码示例。如果您正苦于以下问题:C++ VectorXd::transpose方法的具体用法?C++ VectorXd::transpose怎么用?C++ VectorXd::transpose使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类eigen::VectorXd
的用法示例。
在下文中一共展示了VectorXd::transpose方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: weight_gaussian_predictive_rev
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;
}
示例2: jspace_trivial_path_planner
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;
}
示例3: weight_l2_rev
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);
}
示例4: plan_path_to_predefined_pre_pose
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_;
}
示例5: initialize
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;
}
示例6: getArmConfFull
/* ********************************************************************************************* */
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;
}
示例7: operator
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;
}
示例8: construct_preference_and_covariance
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 ¶meter) {
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();
}
}
示例9: actionStateCallback
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());
}
}
示例10: computePotentials
/** 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);
}
示例11: optimise
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;
}
示例12: grad
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;
}
示例13: main
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;
}
}
示例14: update
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]++;
}
示例15: multivariateGaussianDensity
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));
}