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

  void Line3D::jacobian(Matrix7x6d& Jp, Matrix7x6d& Jl, const Eigen::Isometry3d& x, const Line3D& l){
    Jp.setZero();
    Jl.setZero();
    Matrix6d A=Matrix6d::Zero();
    A.block<3,3>(0,0)=x.linear();
    A.block<3,3>(0,3)= _skew(x.translation())*x.linear();
    A.block<3,3>(3,3)=x.linear();
    Vector6d v=(Vector6d)l;
    Line3D lRemapped(v);

    Matrix6d D = Matrix6d::Zero();
    D.block<3,3>(0,0) = -_skew(l.d());
    D.block<3,3>(0,3) = -2*_skew(l.w());
    D.block<3,3>(3,3) = -2*_skew(l.d());
    Jp.block<6,6>(0,0) = A*D;

    Vector3d d = l.d();
    Vector3d w = l.w();
    double ln = d.norm();
    double iln = 1./ln;
    double iln3 = std::pow(iln,3);
    Matrix6d Jll;
    Jll <<
      iln,0,0,-(w.x()*d.x())*iln3,-(w.x()*d.y())*iln3,-(w.x()*d.z())*iln3,
      0,iln,0,-(w.y()*d.x())*iln3,-(w.y()*d.y())*iln3,-(w.y()*d.z())*iln3,
      0,0,iln,-(w.z()*d.x())*iln3,-(w.z()*d.y())*iln3,-(w.z()*d.z())*iln3,
      0,0,0,-(d.x()*d.x()-ln*ln)*iln3,-(d.x()*d.y())*iln3,-(d.x()*d.z())*iln3,
      0,0,0,-(d.x()*d.y())*iln3,-(d.y()*d.y()-ln*ln)*iln3,-(d.y()*d.z())*iln3,
      0,0,0,-(d.x()*d.z())*iln3,-(d.y()*d.z())*iln3,-(d.z()*d.z()-ln*ln)*iln3;
    Jl.block<6,6>(0,0) = A*Jll;
    Jl.block<1,6>(6,0) << 2*d.x(),2*d.y(),2*d.z();
  }

/**
 * @function setRootTransform
 * @brief Set q (SCREW) from pose <x,y,z,r,p,y> 
 */
void InspectorTab::setRootTransform( dart::dynamics::Skeleton* robot, 
				     const Eigen::Matrix<double, 6, 1>& pose ) {
  dart::dynamics::Joint* joint = robot->getRootBodyNode()->getParentJoint();

  if(dynamic_cast<dart::dynamics::FreeJoint*>(joint)) {
    Matrix<double, 6, 1> q;
    Eigen::Isometry3d transform = Eigen::Isometry3d::Identity();
    transform.translation() = pose.head<3>();
    transform.linear() = dart::math::eulerXYZToMatrix(pose.tail<3>());
    q = dart::math::logMap(transform);
    joint->set_q( q );
  
  }
  else {
    Eigen::Isometry3d transform;
    transform.makeAffine();
    transform.linear() = dart::math::eulerXYZToMatrix(pose.tail<3>());
    transform.translation() = pose.head<3>();
    joint->setTransformFromParentBodyNode(transform);
  }
  
  for (int i = 0; i < robot->getNumBodyNodes(); ++i) {
    robot->getBodyNode(i)->updateTransform();
  }
}

//==============================================================================
Eigen::Isometry3d State::getCOMFrame() const
{
  Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

  // Y-axis
  Eigen::Vector3d yAxis = Eigen::Vector3d::UnitY();

  // X-axis
  Eigen::Vector3d xAxis = mPelvis->getTransform().linear().col(0);
  Eigen::Vector3d pelvisXAxis = mPelvis->getTransform().linear().col(0);
  double mag = yAxis.dot(pelvisXAxis);
  pelvisXAxis -= mag * yAxis;
  xAxis = pelvisXAxis.normalized();

  // Z-axis
  Eigen::Vector3d zAxis = xAxis.cross(yAxis);

  T.translation() = getCOM();

  T.linear().col(0) = xAxis;
  T.linear().col(1) = yAxis;
  T.linear().col(2) = zAxis;

  return T;
}

    bool FLOATING_T::run(robot::robot_state_t& target, control_data_t* data)
    {
        // 1. Setup
        Eigen::Isometry3d body;
        target.get_body(body);

        // 1. Variables
        Eigen::Quaterniond save_rotation(data->joy_rotation);
        Eigen::Vector3d save_position = data->joy_position;
        
        Eigen::Quaterniond rotation(data->joy_rotation);
        Eigen::Vector3d& position = data->joy_position;

        // 2. Clamp to xyy
        position[2] = 0;
        rotation.y() = 0;
        rotation.x() = 0;

        body.linear() = body.linear() * rotation.matrix();
        body.translation() += position;

        // 3.
        target.set_body(body);

        return true;
    }    

 Line3D operator*(const Eigen::Isometry3d& t, const Line3D& line){
   Matrix6d A=Matrix6d::Zero();
   A.block<3,3>(0,0)=t.linear();
   A.block<3,3>(0,3)= _skew(t.translation())*t.linear();
   A.block<3,3>(3,3)=t.linear();
   Vector6d v = (Vector6d)line;
   // cout << "v" << endl << v << endl;
   // cout << "A" << endl << A << endl;
   // cout << "Av" << endl << A*v << endl;
   return Line3D(A*v);
 }

BaseSensorDataNode* SyncSensorDataNodeMaker::makeNode(MapManager* manager, BaseSensorData* data) {
    SynchronizedSensorData* sdata = dynamic_cast<SynchronizedSensorData*>(data);
    if (! sdata)
        return 0;
    SyncSensorDataNode* snode = new SyncSensorDataNode(manager, sdata);
    for (size_t i = 0; i<sdata->sensorDatas.size(); i++) {
        IMUData* imu = dynamic_cast<IMUData*>(sdata->sensorDatas[i]);
        if (imu) {
            MapNodeUnaryRelation* imuRel = new MapNodeUnaryRelation(manager);
            imuRel->nodes()[0] = snode;
            Eigen::Isometry3d t;
            t.setIdentity();
            t.linear() = imu->orientation().toRotationMatrix();
            Eigen::Matrix<double, 6, 6> info;
            info.setZero();
            info.block<3,3>(3,3) = imu->orientationCovariance().inverse();
            //info.block<3,3>(3,3).setIdentity();
            imuRel->setTransform(t);
            imuRel->setInformationMatrix(info);
            snode->setImu(imuRel);
            break;
        }
    }
    return snode;
}

Eigen::Isometry3d getRelativeTransform(TagMatch const& match, Eigen::Matrix3d const & camera_matrix, const Eigen::Vector4d &distortion_coefficients, double tag_size) 
{
  std::vector<cv::Point3f> objPts;
  std::vector<cv::Point2f> imgPts;
  double s = tag_size/2.;
  objPts.push_back(cv::Point3f(-s,-s, 0));
  objPts.push_back(cv::Point3f( s,-s, 0));
  objPts.push_back(cv::Point3f( s, s, 0));
  objPts.push_back(cv::Point3f(-s, s, 0));


  imgPts.push_back(match.p0);
  imgPts.push_back(match.p1);
  imgPts.push_back(match.p2);
  imgPts.push_back(match.p3);

  cv::Mat rvec, tvec;
  cv::Matx33f cameraMatrix(
                           camera_matrix(0,0), 0, camera_matrix(0,2),
                           0, camera_matrix(1,1), camera_matrix(1,2),
                           0,  0,  1);

  cv::Vec4f distParam(distortion_coefficients(0), distortion_coefficients(1), distortion_coefficients(2), distortion_coefficients(3)); 
  cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
  cv::Matx33d r;
  cv::Rodrigues(rvec, r);
  Eigen::Matrix3d wRo;
  wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2);

  Eigen::Isometry3d T; 
  T.linear() = wRo;
  T.translation() << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
  return T;
}

//==============================================================================
Eigen::Vector6d FreeJoint::convertToPositions(const Eigen::Isometry3d& _tf)
{
  Eigen::Vector6d x;
  x.head<3>() = math::logMap(_tf.linear());
  x.tail<3>() = _tf.translation();
  return x;
}

//==============================================================================
btTransform convertTransform(const Eigen::Isometry3d& _T)
{
  btTransform trans;
  trans.setOrigin(convertVector3(_T.translation()));
  trans.setBasis(convertMatrix3x3(_T.linear()));
  return trans;
}

Eigen::Isometry3d toIsometry3d(const std::string& _str)
{
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
    Eigen::Vector6d elements = Eigen::Vector6d::Zero();
    std::vector<std::string> pieces;
    std::string trimedStr = boost::trim_copy(_str);
    boost::split(pieces, trimedStr, boost::is_any_of(" "), boost::token_compress_on);
    assert(pieces.size() == 6);

    for (size_t i = 0; i < pieces.size(); ++i)
    {
        if (pieces[i] != "")
        {
            try
            {
                elements(i) = boost::lexical_cast<double>(pieces[i].c_str());
            }
            catch(boost::bad_lexical_cast& e)
            {
                std::cerr << "value ["
                          << pieces[i]
                          << "] is not a valid double for SE3["
                          << i
                          << "]"
                          << std::endl;
            }
        }
    }

    T.linear() = math::eulerXYZToMatrix(elements.tail<3>());
    T.translation() = elements.head<3>();
    return T;
}

//==============================================================================
void ShapeNode::setRelativeRotation(const Eigen::Matrix3d& rotation)
{
  Eigen::Isometry3d transform = getRelativeTransform();
  transform.linear() = rotation;

  setRelativeTransform(transform);
}

bool RosMessageContext::getOdomPose(Eigen::Isometry3d& _trans, double time){
  bool transformFound = true;
  _tfListener->waitForTransform(_odomReferenceFrameId, _baseReferenceFrameId,
				ros::Time(time), ros::Duration(1.0));
  try{
    tf::StampedTransform t;
    _tfListener->lookupTransform(_odomReferenceFrameId, _baseReferenceFrameId,
				 ros::Time(time), t);
    Eigen::Isometry3d transform;
    transform.translation().x()=t.getOrigin().x();
    transform.translation().y()=t.getOrigin().y();
    transform.translation().z()=t.getOrigin().z();
    Eigen::Quaterniond rot;
    rot.x()=t.getRotation().x();
    rot.y()=t.getRotation().y();
    rot.z()=t.getRotation().z();
    rot.w()=t.getRotation().w();
    transform.linear()=rot.toRotationMatrix();
    _trans = transform;
    transformFound = true;
  }
  catch (tf::TransformException ex){
    ROS_ERROR("%s",ex.what());
    transformFound = false;
  }
  return transformFound;
}

//==============================================================================
fcl::Transform3f FCLTypes::convertTransform(const Eigen::Isometry3d& _T)
{
  fcl::Transform3f trans;

  trans.setTranslation(convertVector3(_T.translation()));
  trans.setRotation(convertMatrix3x3(_T.linear()));

  return trans;
}

Eigen::Vector3d RevoluteJoint::getWorldAxis() const
{
    Eigen::Isometry3d parentTransf = Eigen::Isometry3d::Identity();

    if (this->mParentBodyNode != NULL)
        parentTransf = mParentBodyNode->getWorldTransform();

    return parentTransf.linear() * mT_ParentBodyToJoint.linear() * mAxis;
}

    bool LEG_AIK_T::run(robot::robot_state_t& target, control_data_t* data)
    {
        assert(data);
        
        if(!data->joystick_ok)
            return false;

        // 1. Set up
        kinematics::Skeleton* robot = data->robot;
        robot::robot_kinematics_t* robot_kin = data->kin;
        int ms = data->manip_side;
        int mi = ms ? robot::MANIP_L_FOOT : robot::MANIP_R_FOOT;
        Eigen::Isometry3d Twfoot = data->manip_xform[mi];
        
        Eigen::VectorXd save = target.dart_pose();

        assert(robot);
        assert(robot_kin);
        assert(ms == 1 || ms == 0);

        // 2. Target xform
        Twfoot.linear() = Twfoot.linear() * data->joy_rotation;
        Twfoot.translation() += data->joy_position;

        // 3. Run IK
        bool ok = robot_kin->leg_ik(Twfoot, ms, target);

        if(!ok)
            std::cout << "LEG_AIK: outside workspace\n";

        assert(target.check_limits());

        // 4. Save?
        if (!ok)
            target.set_dart_pose(save);
        else
            data->manip_xform[mi] = Twfoot;

        data->manip_target = Twfoot;

        return ok;
    }