C++ Isometry3d类代码示例

void testDHomogTransInv(int ntests, bool check) {
  Isometry3d T;
  std::default_random_engine generator;
  for (int testnr = 0; testnr < ntests; testnr++) {
    Vector4d q = uniformlyRandomQuat(generator);
//    T = Quaterniond(q(0), q(1), q(2), q(3)) * Translation3d(Vector3d::Random());
    T = Quaterniond(q(0), q(1), q(2), q(3));

    const int nv = 6;
    const int nq = 7;

    auto S = Matrix<double, 6, Dynamic>::Random(6, nv).eval();
    auto qdot_to_v = MatrixXd::Random(nv, nq).eval();

    auto dT = dHomogTrans(T, S, qdot_to_v).eval();
    auto dTInv = dHomogTransInv(T, dT);
    volatile auto vol = dTInv;

    if (check) {
      auto dTInvInv = dHomogTransInv(T.inverse(), dTInv);

      if (!dT.matrix().isApprox(dTInvInv.matrix(), 1e-10)) {
        std::cout << "dTInv:\n" << dTInv << "\n\n";
        std::cout << "dT:\n" << dT << "\n\n";
        std::cout << "dTInvInv:\n" << dTInvInv << "\n\n";
        std::cout << "dTInvInv - dT:\n" << dTInvInv - dT << "\n\n";

        throw std::runtime_error("wrong");
      }
    }
  }
}

void parseInertial(shared_ptr<RigidBody> body, XMLElement* node, RigidBodyTree * model)
{
  Isometry3d T = Isometry3d::Identity();

  XMLElement* origin = node->FirstChildElement("origin");
  if (origin)
    poseAttributesToTransform(origin, T.matrix());

  XMLElement* mass = node->FirstChildElement("mass");
  if (mass)
    parseScalarAttribute(mass, "value", body->mass);

  body->com << T(0, 3), T(1, 3), T(2, 3);

  Matrix<double, TWIST_SIZE, TWIST_SIZE> I = Matrix<double, TWIST_SIZE, TWIST_SIZE>::Zero();
  I.block(3, 3, 3, 3) << body->mass * Matrix3d::Identity();

  XMLElement* inertia = node->FirstChildElement("inertia");
  if (inertia) {
    parseScalarAttribute(inertia, "ixx", I(0, 0));
    parseScalarAttribute(inertia, "ixy", I(0, 1));
    I(1, 0) = I(0, 1);
    parseScalarAttribute(inertia, "ixz", I(0, 2));
    I(2, 0) = I(0, 2);
    parseScalarAttribute(inertia, "iyy", I(1, 1));
    parseScalarAttribute(inertia, "iyz", I(1, 2));
    I(2, 1) = I(1, 2);
    parseScalarAttribute(inertia, "izz", I(2, 2));
  }

  auto bodyI = transformSpatialInertia(T, static_cast<Gradient<Isometry3d::MatrixType, Eigen::Dynamic>::type*>(NULL), I);
  body->I = bodyI.value();
}

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

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

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

Transform3 eigen2Xform(const Isometry3d& B) {
  Eigen::Matrix4d me = B.matrix();
  mat4_t<real> mz;
  for (int i=0; i<4; ++i) {
    for (int j=0; j<4; ++j) {
      mz(i,j) = me(i,j);
    }
  }
  Transform3 rval;
  rval.setFromMatrix(mz);
  return rval;
}

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

Pointcloud::Ptr joint(camera &cam, Pointcloud::Ptr points, Isometry3d &T, frame &newframe)
{
	Pointcloud::Ptr cloud = map2point(cam, newframe.rgb, newframe.depth);
  
  cout << "combining clouds together" << endl;
  Pointcloud::Ptr output(new Pointcloud());
  //transform cloud points into the same coordinate system
  transformPointCloud(*points, *output, T.matrix());
  //put the points together
  *output += *cloud;
  
  return output;
}

DLL_EXPORT_SYM
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
  if (nrhs != 6 || nlhs != 7) {
    mexErrMsgIdAndTxt("Drake:testGeometryGradientsmex:BadInputs",
                      "Usage [dT, dTInv, dAdT, dAdT_transpose, x_norm, "
                      "dx_norm, ddx_norm] = testGeometryGradientsmex(T, S, "
                      "qdot_to_v, X, dX, x)");
  }

  int argnum = 0;
  Isometry3d T;
  memcpy(T.data(), mxGetPr(prhs[argnum++]),
         sizeof(double) * drake::kHomogeneousTransformSize);
  auto S = matlabToEigen<drake::kTwistSize, Eigen::Dynamic>(prhs[argnum++]);
  auto qdot_to_v =
      matlabToEigen<Eigen::Dynamic, Eigen::Dynamic>(prhs[argnum++]);
  auto X = matlabToEigen<drake::kTwistSize, Eigen::Dynamic>(prhs[argnum++]);
  auto dX = matlabToEigen<Eigen::Dynamic, Eigen::Dynamic>(prhs[argnum++]);
  auto x = matlabToEigen<4, 1>(prhs[argnum++]);

  auto dT = dHomogTrans(T, S, qdot_to_v).eval();
  auto dTInv = dHomogTransInv(T, dT).eval();
  auto dAdT = dTransformSpatialMotion(T, X, dT, dX).eval();
  auto dAdTInv_transpose = dTransformSpatialForce(T, X, dT, dX).eval();

  Vector4d x_norm;
  Gradient<Vector4d, 4, 1>::type dx_norm;
  Gradient<Vector4d, 4, 2>::type ddx_norm;
  drake::math::NormalizeVector(x, x_norm, &dx_norm, &ddx_norm);

  int outnum = 0;
  plhs[outnum++] = eigenToMatlab(dT);
  plhs[outnum++] = eigenToMatlab(dTInv);
  plhs[outnum++] = eigenToMatlab(dAdT);
  plhs[outnum++] = eigenToMatlab(dAdTInv_transpose);
  plhs[outnum++] = eigenToMatlab(x_norm);
  plhs[outnum++] = eigenToMatlab(dx_norm);
  plhs[outnum++] = eigenToMatlab(ddx_norm);
}

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

}