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

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

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

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

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

  // translational part
  body_pose_des.translation() = xyzexp.head<3>();
  xyzdot_angular_vel.head<3>() = xyzexpdot.head<3>();
  xyzddot_angular_accel.head<3>() = xyzexpddot.head<3>();

  // rotational part
  auto expmap = xyzexp.tail<3>();
  auto expmap_dot = xyzexpdot.tail<3>();
  auto expmap_ddot = xyzexpddot.tail<3>();

  // construct autodiff version of expmap
  // autodiff derivatives represent first and second derivative w.r.t. time
  // TODO(tkoolen): should use 1 instead of dynamic, but causes issues
  // with eigen on MSVC 32 bit; should be fixed in 3.3
  typedef AutoDiffScalar<Matrix<double, Dynamic, 1>> ADScalar;
  // TODO(tkoolen): should use 1 instead of dynamic, but causes issues
  // with eigen on MSVC 32 bit; should be fixed in 3.3
  typedef AutoDiffScalar<Matrix<ADScalar, Dynamic, 1>> ADScalarSecondDeriv;
  Matrix<ADScalarSecondDeriv, 3, 1> expmap_autodiff;
  for (int i = 0; i < expmap_autodiff.size(); i++) {
    expmap_autodiff(i).value() = expmap(i);
    expmap_autodiff(i).derivatives().resize(1);
    expmap_autodiff(i).derivatives()(0) = expmap_dot(i);
    expmap_autodiff(i).derivatives()(0).derivatives().resize(1);
    expmap_autodiff(i).derivatives()(0).derivatives()(0) = expmap_ddot(i);
  }

  auto quat_autodiff = expmap2quat(expmap_autodiff);
  Vector4d quat = autoDiffToValueMatrix(autoDiffToValueMatrix(quat_autodiff));
  body_pose_des.linear() = quat2rotmat(quat);

  // angular velocity and acceleration are computed from quaternion derivative
  // meaning of derivative vectors remains the same: first and second
  // derivatives w.r.t. time
  decltype(quat_autodiff) quat_dot_autodiff;
  for (int i = 0; i < quat_dot_autodiff.size(); i++) {
    quat_dot_autodiff(i).value() = quat_autodiff(i).derivatives()(0).value();
    quat_dot_autodiff(i).derivatives().resize(1);
    quat_dot_autodiff(i).derivatives()(0).value() =
        quat_autodiff(i).derivatives()(0).derivatives()(0);
    quat_dot_autodiff(i).derivatives()(0).derivatives().resize(1);
    quat_dot_autodiff(i).derivatives()(0).derivatives()(0) =
        std::numeric_limits<double>::quiet_NaN();  // we're not interested in
                                                   // second deriv of angular
                                                   // velocity
  }

  auto omega_autodiff =
      (quatdot2angularvelMatrix(quat_autodiff) * quat_dot_autodiff).eval();
  auto omega = xyzdot_angular_vel.tail<3>();
  auto omega_dot = xyzddot_angular_accel.tail<3>();
  for (int i = 0; i < omega_autodiff.size(); i++) {
    omega(i) = omega_autodiff(i).value().value();
    omega_dot(i) = omega_autodiff(i).derivatives()(0).value();
  }
}

Vector6d bodySpatialMotionPD(
    const RigidBodyTree &r, const DrakeRobotState &robot_state,
    const int body_index, const Isometry3d &body_pose_des,
    const Ref<const Vector6d> &body_v_des,
    const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp,
    const Ref<const Vector6d> &Kd, const Isometry3d &T_task_to_world) {
  // @param body_pose_des  desired pose in the task frame, this is the
  // homogeneous transformation from desired body frame to task frame
  // @param body_v_des    desired [xyzdot;angular_velocity] in task frame
  // @param body_vdot_des    desired [xyzddot;angular_acceleration] in task
  // frame
  // @param Kp     The gain in task frame
  // @param Kd     The gain in task frame
  // @param T_task_to_world  The homogeneous transform from task to world
  // @retval twist_dot, [angular_acceleration, xyz_acceleration] in body frame

  Isometry3d T_world_to_task = T_task_to_world.inverse();
  KinematicsCache<double> cache = r.doKinematics(robot_state.q, robot_state.qd);

  auto body_pose = r.relativeTransform(cache, 0, body_index);
  const auto &body_xyz = body_pose.translation();
  Vector3d body_xyz_task = T_world_to_task * body_xyz;
  Vector4d body_quat = rotmat2quat(body_pose.linear());
  std::vector<int> v_indices;
  auto J_geometric =
      r.geometricJacobian(cache, 0, body_index, body_index, true, &v_indices);
  VectorXd v_compact(v_indices.size());
  for (size_t i = 0; i < v_indices.size(); i++) {
    v_compact(i) = robot_state.qd(v_indices[i]);
  }
  Vector6d body_twist = J_geometric * v_compact;
  Matrix3d R_body_to_world = quat2rotmat(body_quat);
  Matrix3d R_world_to_body = R_body_to_world.transpose();
  Matrix3d R_body_to_task = T_world_to_task.linear() * R_body_to_world;
  Vector3d body_angular_vel =
      R_body_to_world *
      body_twist.head<3>();  // body_angular velocity in world frame
  Vector3d body_xyzdot =
      R_body_to_world * body_twist.tail<3>();  // body_xyzdot in world frame
  Vector3d body_angular_vel_task = T_world_to_task.linear() * body_angular_vel;
  Vector3d body_xyzdot_task = T_world_to_task.linear() * body_xyzdot;

  Vector3d body_xyz_des = body_pose_des.translation();
  Vector3d body_angular_vel_des = body_v_des.tail<3>();
  Vector3d body_angular_vel_dot_des = body_vdot_des.tail<3>();

  Vector3d xyz_err_task = body_xyz_des - body_xyz_task;

  Matrix3d R_des = body_pose_des.linear();
  Matrix3d R_err_task = R_des * R_body_to_task.transpose();
  Vector4d angleAxis_err_task = rotmat2axis(R_err_task);
  Vector3d angular_err_task =
      angleAxis_err_task.head<3>() * angleAxis_err_task(3);

  Vector3d xyzdot_err_task = body_v_des.head<3>() - body_xyzdot_task;
  Vector3d angular_vel_err_task = body_angular_vel_des - body_angular_vel_task;

  Vector3d Kp_xyz = Kp.head<3>();
  Vector3d Kd_xyz = Kd.head<3>();
  Vector3d Kp_angular = Kp.tail<3>();
  Vector3d Kd_angular = Kd.tail<3>();
  Vector3d body_xyzddot_task =
      (Kp_xyz.array() * xyz_err_task.array()).matrix() +
      (Kd_xyz.array() * xyzdot_err_task.array()).matrix() +
      body_vdot_des.head<3>();
  Vector3d body_angular_vel_dot_task =
      (Kp_angular.array() * angular_err_task.array()).matrix() +
      (Kd_angular.array() * angular_vel_err_task.array()).matrix() +
      body_angular_vel_dot_des;

  Vector6d twist_dot = Vector6d::Zero();
  Vector3d body_xyzddot = T_task_to_world.linear() * body_xyzddot_task;
  Vector3d body_angular_vel_dot =
      T_task_to_world.linear() * body_angular_vel_dot_task;
  twist_dot.head<3>() = R_world_to_body * body_angular_vel_dot;
  twist_dot.tail<3>() = R_world_to_body * body_xyzddot -
                        body_twist.head<3>().cross(body_twist.tail<3>());
  return twist_dot;
}

Vector6d bodySpatialMotionPD(RigidBodyManipulator *r, DrakeRobotState &robot_state, const int body_index, const Isometry3d &body_pose_des, const Ref<const Vector6d> &body_v_des, const Ref<const Vector6d> &body_vdot_des, const Ref<const Vector6d> &Kp, const Ref<const Vector6d> &Kd, const Isometry3d &T_task_to_world)
{
  // @param body_pose_des  desired pose in the task frame, this is the homogeneous transformation from desired body frame to task frame 
  // @param body_v_des    desired [xyzdot;angular_velocity] in task frame
  // @param body_vdot_des    desired [xyzddot;angular_acceleration] in task frame
  // @param Kp     The gain in task frame
  // @param Kd     The gain in task frame 
  // @param T_task_to_world  The homogeneous transform from task to world
  // @retval twist_dot, [angular_acceleration, xyz_acceleration] in body frame

  if(!r->getUseNewKinsol())
  {
    throw std::runtime_error("bodySpatialMotionPD requires new kinsol format");
  }
  Isometry3d T_world_to_task = T_task_to_world.inverse();
  r->doKinematicsNew(robot_state.q,robot_state.qd,false);

  Vector3d origin = Vector3d::Zero();
  auto body_pose = r->forwardKinNew(origin,body_index,0,2,0);
  Vector3d body_xyz = body_pose.value().head<3>();
  Vector3d body_xyz_task = T_world_to_task * body_xyz.colwise().homogeneous();
  Vector4d body_quat = body_pose.value().tail<4>();
  std::vector<int> v_indices;
  auto J_geometric = r->geometricJacobian<double>(0,body_index,body_index,0,true,&v_indices);
  VectorXd v_compact(v_indices.size());
  for(size_t i = 0;i<v_indices.size();i++)
  {
    v_compact(i) = robot_state.qd(v_indices[i]);
  }
  Vector6d body_twist = J_geometric.value() * v_compact;
  Matrix3d R_body_to_world = quat2rotmat(body_quat);
  Matrix3d R_world_to_body = R_body_to_world.transpose();
  Matrix3d R_body_to_task = T_world_to_task.linear() * R_body_to_world;
  Vector3d body_angular_vel = R_body_to_world * body_twist.head<3>();// body_angular velocity in world frame
  Vector3d body_xyzdot = R_body_to_world * body_twist.tail<3>();// body_xyzdot in world frame
  Vector3d body_angular_vel_task = T_world_to_task.linear() * body_angular_vel;
  Vector3d body_xyzdot_task = T_world_to_task.linear() * body_xyzdot;

  Vector3d body_xyz_des = body_pose_des.translation();
  Vector3d body_angular_vel_des = body_v_des.tail<3>();
  Vector3d body_angular_vel_dot_des = body_vdot_des.tail<3>();

  Vector3d xyz_err_task = body_xyz_des-body_xyz_task;

  Matrix3d R_des = body_pose_des.linear();
  Matrix3d R_err_task = R_des * R_body_to_task.transpose();
  Vector4d angleAxis_err_task = rotmat2axis(R_err_task); 
  Vector3d angular_err_task = angleAxis_err_task.head<3>() * angleAxis_err_task(3);

  Vector3d xyzdot_err_task = body_v_des.head<3>() - body_xyzdot_task;
  Vector3d angular_vel_err_task = body_angular_vel_des - body_angular_vel_task;

  Vector3d Kp_xyz = Kp.head<3>();
  Vector3d Kd_xyz = Kd.head<3>();
  Vector3d Kp_angular = Kp.tail<3>();
  Vector3d Kd_angular = Kd.tail<3>();
  Vector3d body_xyzddot_task = (Kp_xyz.array() * xyz_err_task.array()).matrix() + (Kd_xyz.array() * xyzdot_err_task.array()).matrix() + body_vdot_des.head<3>();
  Vector3d body_angular_vel_dot_task = (Kp_angular.array() * angular_err_task.array()).matrix() + (Kd_angular.array() * angular_vel_err_task.array()).matrix() + body_angular_vel_dot_des;
  
  Vector6d twist_dot = Vector6d::Zero();
  Vector3d body_xyzddot = T_task_to_world.linear() * body_xyzddot_task;
  Vector3d body_angular_vel_dot = T_task_to_world.linear() * body_angular_vel_dot_task;
  twist_dot.head<3>() = R_world_to_body * body_angular_vel_dot;
  twist_dot.tail<3>() = R_world_to_body * body_xyzddot - body_twist.head<3>().cross(body_twist.tail<3>());
  return twist_dot;
}

bool AlignmentAlgorithmPlaneLinear::operator()(AlignmentAlgorithmPlaneLinear::TransformType& transform, const CorrespondenceVector& correspondences, const IndexVector& indices)
{
    double err=0;
    //If my correspondaces indices are less then a minimum threshold, stop it please
    if ((int)indices.size()<minimalSetSize()) return false;

    //My initial guess for the transformation it's the identity matrix
    //maybe in the future i could have a less rough guess
    transform = Isometry3d::Identity();
    //Unroll the matrix to a vector
    Vector12d x=homogeneous2vector(transform.matrix());
    Matrix9x1d rx=x.block<9,1>(0,0);

    //Initialization of variables, melting fat, i've so much space
    Matrix9d H;
    H.setZero();
    Vector9d b;
    b.setZero();
    Matrix3x9d A;
    //iteration for each correspondace
    for (size_t i=0; i<indices.size(); i++)
    {

        A.setZero();
        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();
        Vector4d coeffs_j=pj.toVector();

        ni=coeffs_i.head(3);
        nj=coeffs_j.head(3);

        //inizializzo lo jacobiano, solo la parte rotazionale (per ora)
        A.block<1,3>(0,0)=nj.transpose();
        A.block<1,3>(1,3)=nj.transpose();
        A.block<1,3>(2,6)=nj.transpose();

        if(DEBUG) {
            cerr << "[DEBUG] PI"<<endl;
            cerr << coeffs_i<<endl;
            cerr << "[DEBUG] PJ "<<endl;
            cerr << coeffs_j<<endl;
            cerr << "[ROTATION JACOBIAN][PLANE "<<i<<"]"<<endl;
            cerr << A<<endl;
        }
        //errore
        //inizializzo errore
        Vector3d ek;
        ek.setZero();
        ek=A*x.block<9,1>(0,0)-coeffs_i.head(3);
        H+=A.transpose()*A;

        b+=A.transpose()*ek;

        err+=abs(ek.dot(ek));

        if(DEBUG)
            cerr << "[ROTATIONAL Hessian for plane "<<i<<"]"<<endl<<H<<endl;
        if(DEBUG)
            cerr << "[ROTATIONAL B for plane "<<i<<"]"<<endl<<b<<endl;
    }
    LDLT<Matrix9d> ldlt(H);
    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

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