C++ Isometry3d::translation方法代码示例

  void EdgeSE3OdomDifferentialCalib::computeError()
  {
      const VertexSE3* v1                        = dynamic_cast<const VertexSE3*>(_vertices[0]);
      const VertexSE3* v2                        = dynamic_cast<const VertexSE3*>(_vertices[1]);
      const VertexOdomParams* params = dynamic_cast<const VertexOdomParams*>(_vertices[2]);
      const Isometry3d& x1                              = v1->estimate();
      const Isometry3d& x2                              = v2->estimate();

      Isometry3d odom = internal::fromVectorMQT(measurement());
      Eigen::Vector3d rpy = g2o::internal::toEuler(odom.linear());

      //        g2o::MotionMeasurement mma(odom.translation()(0), odom.translation()(1), rpy(2), diff_time_);
      //        g2o::VelocityMeasurement vel = g2o::OdomConvert::convertToVelocity(mma);
      //        vel.setVl(params->estimate()(0) * vel.vl());
      //        vel.setVr(params->estimate()(1) * vel.vr());
      //        g2o::MotionMeasurement mmb = g2o::OdomConvert::convertToMotion(vel, params->estimate()(2));
      //        odom.translation()(0) = mmb.x();
      //        odom.translation()(1) = mmb.y();
      //        rpy(2) = mmb.theta();
      //        odom.linear() = g2o::internal::fromEuler(rpy);

      // move to cpp file
      // implement translation scale
      // implement rotation scale, which affects odometry translation

      double drift_theta = params->estimate()(1) * fabs(rpy(2));
      double drift_trans = params->estimate()(2) * odom.translation().norm();
      Eigen::AngleAxisd drift_rot(drift_theta + drift_trans, Eigen::Vector3d::UnitZ());
      odom.translation() = params->estimate()(0) * (drift_rot * odom.translation());
      rpy(2) += drift_theta + drift_trans;
      odom.linear() = g2o::internal::fromEuler(rpy);

      Isometry3d delta = x2.inverse() * x1 * odom;
      _error = internal::toVectorMQT(delta);
  }

Isometry3d PrismaticJoint::jointTransform(double* const q) const
{
  Isometry3d ret;
  ret.linear().setIdentity();
  ret.translation() = q[0] * translation_axis;
  ret.makeAffine();
  return ret;
}

Eigen::Isometry3d DecodePose(const bot_core::position_3d_t& msg) {
  Isometry3d ret;
  ret.translation() = DecodeVector3d(msg.translation);
  auto quaternion = DecodeQuaternion(msg.rotation);
  ret.linear() = drake::math::quat2rotmat(quaternion);
  ret.makeAffine();
  return ret;
}

 Vector7d toVectorQT(const Isometry3d& t){
   Quaterniond q(extractRotation(t));
   q.normalize();
   Vector7d v;
   v[3] = q.x(); v[4] = q.y(); v[5] = q.z(); v[6] = q.w();
   v.block<3,1>(0,0) = t.translation();
   return v;
 }

Isometry3d RollPitchYawFloatingJoint::jointTransform(const Eigen::Ref<const VectorXd>& q) const
{
  Isometry3d ret;
  auto pos = q.middleRows<SPACE_DIMENSION>(0);
  auto rpy = q.middleRows<RPY_SIZE>(SPACE_DIMENSION);
  ret.linear() = rpy2rotmat(rpy);
  ret.translation() = pos;
  ret.makeAffine();
  return ret;
}

Isometry3d RollPitchYawFloatingJoint::jointTransform(double* const q) const
{
  Isometry3d ret;
  Map<Vector3d> pos(&q[0]);
  Map<Vector3d> rpy(&q[3]);
  ret.linear() = rpy2rotmat(rpy);
  ret.translation() = pos;
  ret.makeAffine();
  return ret;
}

//==============================================================================
Isometry3d getRandomTransform()
{
  Isometry3d T = Isometry3d::Identity();

  const Vector3d rotation = math::randomVector<3>(-DART_PI, DART_PI);
  const Vector3d position = Vector3d(math::random(-1.0, 1.0),
                                     math::random( 0.5, 1.0),
                                     math::random(-1.0, 1.0));

  T.translation() = position;
  T.linear() = math::expMapRot(rotation);

  return T;
}

void computeEdgeSE3PriorGradient(Isometry3d& E,
                                 Matrix6d& J, 
                                 const Isometry3d& Z, 
                                 const Isometry3d& X,
                                 const Isometry3d& P){
  // compute the error at the linearization point
  
  const Isometry3d A = Z.inverse()*X;
  const Isometry3d B = P;
  const Matrix3d Ra = A.rotation();
  const Matrix3d Rb = B.rotation();
  const Vector3d tb = B.translation();
  E = A*B;
  const Matrix3d Re=E.rotation();
  
  Matrix<double, 3 , 9 >  dq_dR;
  compute_dq_dR (dq_dR, 
     Re(0,0),Re(1,0),Re(2,0),
     Re(0,1),Re(1,1),Re(2,1),
     Re(0,2),Re(1,2),Re(2,2));

  J.setZero();
  
  // dte/dt
  J.block<3,3>(0,0)=Ra;

  // dte/dq =0
  // dte/dqj
  {
    Matrix3d S;
    skew(S,tb);
    J.block<3,3>(0,3)=Ra*S;
  }

  // dre/dt =0
  
  // dre/dq
  {
    double buf[27];
    Map<Matrix<double, 9,3> > M(buf);
    Matrix3d Sx,Sy,Sz;
    skew(Sx,Sy,Sz,Rb);
    Map<Matrix3d> Mx(buf);    Mx = Ra*Sx;
    Map<Matrix3d> My(buf+9);  My = Ra*Sy;
    Map<Matrix3d> Mz(buf+18); Mz = Ra*Sz;
    J.block<3,3>(3,3) = dq_dR * M;
  }

}

void CollisionInterface::updateBodyNodes() {
    int numNodes = mNodeMap.size();
    for (std::map<BodyNode*, RigidBody*>::iterator it = mNodeMap.begin(); it != mNodeMap.end(); ++it) {
        BodyNode *bn = it->first;
        RigidBody *rb = it->second;
        if (bn->getSkeleton()->getName() == "pinata")
            continue;
        Isometry3d W;
        W.setIdentity();
        W.linear() = rb->mOrientation;
        W.translation() = rb->mPosition;
        W.makeAffine();
        bn->getSkeleton()->getJoint("freeJoint")->setTransformFromParentBodyNode(W);
        //bn->updateTransform();
        bn->getSkeleton()->computeForwardKinematics(true, false, false);
    }
}

void evaluateXYZExpmapCubicSpline(double t, const PiecewisePolynomial<double> &spline, Isometry3d &body_pose_des, Vector6d &xyzdot_angular_vel, Vector6d &xyzddot_angular_accel) {
  Vector6d xyzexp = spline.value(t);
  auto derivative = spline.derivative();
  Vector6d xyzexpdot = derivative.value(t);
  Vector6d xyzexpddot = derivative.derivative().value(t);
  xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>();
  xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>();
  Vector3d expmap = xyzexp.tail<3>();
  auto quat_grad = expmap2quat(expmap,2);
  Vector4d quat = quat_grad.value();
  body_pose_des.linear() = quat2rotmat(quat);
  body_pose_des.translation() = xyzexp.head<3>();
  Vector4d quat_dot = quat_grad.gradient().value() * xyzexpdot.tail<3>();
  quat_grad.gradient().gradient().value().resize(12,3);
  Matrix<double,12,3> dE = quat_grad.gradient().gradient().value();
  Vector3d expdot = xyzexpdot.tail<3>();
  Matrix<double,4,3> Edot = matGradMult(dE,expdot);
  Vector4d quat_ddot = quat_grad.gradient().value()*xyzexpddot.tail<3>() + Edot*expdot;
  Matrix<double,3,4> M;
  Matrix<double,12,4> dM;
  quatdot2angularvelMatrix(quat,M,&dM);
  xyzdot_angular_vel.tail<3>() = M*quat_dot;
  xyzddot_angular_accel.tail<3>() = M*quat_ddot + matGradMult(dM,quat_dot)*quat_dot;
}

void computeEdgeSE3Gradient(Isometry3d& E,
                        Matrix6d& Ji, 
                        Matrix6d& Jj,
                        const Isometry3d& Z, 
                        const Isometry3d& Xi,
                        const Isometry3d& Xj,
                        const Isometry3d& Pi, 
                        const Isometry3d& Pj
                        ){
  // compute the error at the linearization point
  const Isometry3d A=Z.inverse()*Pi.inverse();
  const Isometry3d B=Xi.inverse()*Xj;
  const Isometry3d C=Pj;
 
  const Isometry3d AB=A*B;  
  const Isometry3d BC=B*C;
  E=AB*C;

  const Matrix3d Re=E.rotation();
  const Matrix3d Ra=A.rotation();
  const Matrix3d Rb=B.rotation();
  const Matrix3d Rc=C.rotation();
  const Vector3d tc=C.translation();
  //const Vector3d tab=AB.translation();
  const Matrix3d Rab=AB.rotation();
  const Vector3d tbc=BC.translation();  
  const Matrix3d Rbc=BC.rotation();

  Matrix<double, 3 , 9 >  dq_dR;
  compute_dq_dR (dq_dR, 
     Re(0,0),Re(1,0),Re(2,0),
     Re(0,1),Re(1,1),Re(2,1),
     Re(0,2),Re(1,2),Re(2,2));

  Ji.setZero();
  Jj.setZero();
  
  // dte/dti
  Ji.block<3,3>(0,0)=-Ra;

  // dte/dtj
  Jj.block<3,3>(0,0)=Ra*Rb;

  // dte/dqi
  {
    Matrix3d S;
    skewT(S,tbc);
    Ji.block<3,3>(0,3)=Ra*S;
  }

  // dte/dqj
  {
    Matrix3d S;
    skew(S,tc);
    Jj.block<3,3>(0,3)=Rab*S;
  }

  // dre/dqi
  {
    double buf[27];
    Map<Matrix<double, 9,3> > M(buf);
    Matrix3d Sxt,Syt,Szt;
    skewT(Sxt,Syt,Szt,Rbc);
    Map<Matrix3d> Mx(buf);    Mx = Ra*Sxt;
    Map<Matrix3d> My(buf+9);  My = Ra*Syt;
    Map<Matrix3d> Mz(buf+18); Mz = Ra*Szt;
    Ji.block<3,3>(3,3) = dq_dR * M;
  }

  // dre/dqj
  {
    double buf[27];
    Map<Matrix<double, 9,3> > M(buf);
    Matrix3d Sx,Sy,Sz;
    skew(Sx,Sy,Sz,Rc);
    Map<Matrix3d> Mx(buf);    Mx = Rab*Sx;
    Map<Matrix3d> My(buf+9);  My = Rab*Sy;
    Map<Matrix3d> Mz(buf+18); Mz = Rab*Sz;
    Jj.block<3,3>(3,3) = dq_dR * M;
  }

}

rk_result_t Robot::dampedLeastSquaresIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues, const Isometry3d &target, const Isometry3d &finalTF)
{


    vector<Linkage::Joint*> joints;
    joints.resize(jointIndices.size());
    // FIXME: Add in safety checks
    for(int i=0; i<joints.size(); i++)
        joints[i] = joints_[jointIndices[i]];

    // ~~ Declarations ~~
    MatrixXd J;
    MatrixXd Jinv;
    Isometry3d pose;
    AngleAxisd aagoal(target.rotation());
    AngleAxisd aastate;
    Vector3d Terr;
    Vector3d Rerr;
    Vector6d err;
    VectorXd delta(jointValues.size());
    VectorXd f(jointValues.size());


    tolerance = 0.001;
    maxIterations = 50; // TODO: Put this in the constructor so the user can set it arbitrarily
    damp = 0.05;

    values(jointIndices, jointValues);

    pose = joint(jointIndices.back()).respectToRobot()*finalTF;
    aastate = pose.rotation();

    Terr = target.translation()-pose.translation();
    Rerr = aagoal.angle()*aagoal.axis()-aastate.angle()*aastate.axis();
    err << Terr, Rerr;

    size_t iterations = 0;
    do {

        jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);

        f = (J*J.transpose() + damp*damp*Matrix6d::Identity()).colPivHouseholderQr().solve(err);
        delta = J.transpose()*f;

        jointValues += delta;

        values(jointIndices, jointValues);

        pose = joint(jointIndices.back()).respectToRobot()*finalTF;
        aastate = pose.rotation();

        Terr = target.translation()-pose.translation();
        Rerr = aagoal.angle()*aagoal.axis()-aastate.angle()*aastate.axis();
        err << Terr, Rerr;

        iterations++;


    } while(err.norm() > tolerance && iterations < maxIterations);

}

rk_result_t Robot::jacobianTransposeIK_chain(const vector<size_t> &jointIndices, VectorXd &jointValues, const Isometry3d &target, const Isometry3d &finalTF)
{
    return RK_SOLVER_NOT_READY;
    // FIXME: Make this solver work


    vector<Linkage::Joint*> joints;
    joints.resize(jointIndices.size());
    // FIXME: Add in safety checks
    for(int i=0; i<joints.size(); i++)
        joints[i] = joints_[jointIndices[i]];

    // ~~ Declarations ~~
    MatrixXd J;
    MatrixXd Jinv;
    Isometry3d pose;
    AngleAxisd aagoal;
    AngleAxisd aastate;
    Vector6d state;
    Vector6d err;
    VectorXd delta(jointValues.size());
    Vector6d gamma;
    double alpha;

    aagoal = target.rotation();

    double Tscale = 3; // TODO: Put these as a class member in the constructor
    double Rscale = 0;

    tolerance = 1*M_PI/180; // TODO: Put this in the constructor so the user can set it arbitrarily
    maxIterations = 100; // TODO: Put this in the constructor so the user can set it arbitrarily

    size_t iterations = 0;
    do {
        values(jointIndices, jointValues);

        jacobian(J, joints, joints.back()->respectToRobot().translation()+finalTF.translation(), this);

        pose = joint(jointIndices.back()).respectToRobot()*finalTF;
        aastate = pose.rotation();
        state << pose.translation(), aastate.axis()*aastate.angle();

        err << (target.translation()-pose.translation()).normalized()*Tscale,
               (aagoal.angle()*aagoal.axis()-aastate.angle()*aastate.axis()).normalized()*Rscale;

        gamma = J*J.transpose()*err;
        alpha = err.dot(gamma)/gamma.norm();

        delta = alpha*J.transpose()*err;

        jointValues += delta;
        iterations++;

        std::cout << iterations << " | Norm:" << delta.norm()
//                  << "\tdelta: " << delta.transpose() << "\tJoints:" << jointValues.transpose() << std::endl;
                  << " | " << (target.translation() - pose.translation()).norm()
                  << "\tErr: " << (target.translation()-pose.translation()).transpose() << std::endl;

    } while(err.norm() > tolerance && iterations < maxIterations);

}

 // functions to handle the toVector of the whole transformations
 Vector6d toVectorMQT(const Isometry3d& t) {
   Vector6d v;
   v.block<3,1>(3,0) = toCompactQuaternion(extractRotation(t));
   v.block<3,1>(0,0) = t.translation();
   return v;
 }

//.........这里部分代码省略.........
    if (ldlt.isNegative()) return false;
    rx=ldlt.solve(-b); // using a LDLT factorizationldlt;

    x.block<3,1>(0,0)+=rx.block<3,1>(0,0);
    x.block<3,1>(3,0)+=rx.block<3,1>(3,0);
    x.block<3,1>(6,0)+=rx.block<3,1>(6,0);
    if(DEBUG) {
        cerr << "Solving the linear system"<<endl;
        cerr << "------------>H"<<endl;
        cerr << H<<endl;
        cerr << "------------>b"<<endl;
        cerr << b<<endl;
        cerr << "------------>rx"<<endl;
        cerr << rx<<endl;
        cerr << "------------>xTEMP"<<endl;
        cerr << vector2homogeneous(x)<<endl<<endl;

        cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
        cerr << "łłłłłłłłłłł Ringraziamo Cthulhu la parte rotazionale è finitałłłłłłłłłłł"<<endl;
        cerr << "łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł łłłłłłłłłłł"<<endl;
    }
    Matrix4d Xtemp=vector2homogeneous(x);

    //now the problem si solved but i could have (and probably i have!)
    //a not orthogonal rotation matrix, so i've to recondition it

    Matrix3x3d R=Xtemp.block<3,3>(0,0);
    // recondition the rotation
    JacobiSVD<Matrix3x3d> svd(R, Eigen::ComputeThinU | Eigen::ComputeThinV);
    if (svd.singularValues()(0)<.5) return false;
    R=svd.matrixU()*svd.matrixV().transpose();
    Isometry3d X = Isometry3d::Identity();
    X.linear()=R;
    X.translation()= x.block<3,1>(0,3);
    if(DEBUG)
        cerr << "X dopo il ricondizionamento appare come:"<<endl;
    if(DEBUG)
        cerr << X.matrix()<<endl;


    Matrix3d H2=Matrix3d::Zero();
    Vector3d b2=Vector3d::Zero();
    Vector3d A2=Vector3d::Zero();

    //at this point rotation is cured, now it's time to work on the translation

    for (size_t i=0; i<indices.size(); i++)
    {
        if(DEBUG)
            cerr << "[TRANSLATION JACOBIAN START][PLANE "<<i<<"]"<<endl;

        const Correspondence& c = correspondences[indices[i]];
        const EdgePlane* edge = static_cast<const EdgePlane*>(c.edge());

        //estraggo i vertici dagli edge
        const VertexPlane* v1 = static_cast<const VertexPlane*>(edge->vertex(0));
        const VertexPlane* v2 = static_cast<const VertexPlane*>(edge->vertex(1));

        //recupero i dati dei piani
        const AlignmentAlgorithmPlaneLinear::PointEstimateType& pi= v1->estimate();
        const AlignmentAlgorithmPlaneLinear::PointEstimateType& pj= v2->estimate();

        //recupeo le normali, mi servono per condizionare la parte rotazionale
        Vector3d ni;
        Vector3d nj;
        Vector4d coeffs_i=pi.toVector();