C++ Matrix::setIdentity方法代码示例

void LipmVarHeightPlanner::getAB(const double x0[6], const double u[3], double z0, Eigen::Matrix<double,6,6> &A, Eigen::Matrix<double,6,3> &B) const
{
  A.setIdentity();
  B.setZero();
 
  double x = x0[XX];
  double y = x0[YY];
  double z = x0[ZZ] - z0;

  double px = u[XX];
  double py = u[YY];
  double F = u[ZZ];

  // A
  A(0, 3) = _dt;
  A(1, 4) = _dt;
  A(2, 5) = _dt;

  A(3, 0) = F*_dt/(_mass*z);
  A(3, 2) = F*_dt*(px-x)/(_mass*z*z);

  A(4, 1) = F*_dt/(_mass*z);
  A(4, 2) = F*_dt*(py-y)/(_mass*z*z);
 
  // B
  B(3, 0) = -F*_dt/(_mass*z); 
  B(3, 2) = -(px-x)*_dt/(_mass*z); 
  
  B(4, 1) = -F*_dt/(_mass*z); 
  B(4, 2) = -(py-y)*_dt/(_mass*z);

  B(5, 2) = _dt/_mass; 
}

void SensorDataNodeMaker::process(Serializable* s) {
    put(s);
    BaseSensorData* data = dynamic_cast<BaseSensorData*>(s);
    if (data && data->topic() == _topic) {
        BaseSensorDataNode* currentNode = makeNode(_mapManager, data);
        Eigen::Isometry3d currentNodeTransform = data->robotReferenceFrame()->transform();
        currentNode->setTransform(currentNodeTransform);
        _mapManager->addNode(currentNode);
        MapNodeBinaryRelation* odom = 0;
        if (_previousNode) {
            odom = new MapNodeBinaryRelation(_mapManager);
            odom->nodes()[0]=_previousNode;
            odom->nodes()[1]=currentNode;
            odom->setTransform(_previousNodeTransform.inverse()*currentNodeTransform);
            Eigen::Matrix<double, 6, 6> info;
            info.setIdentity();
            info.block<3,3>(3,3) *= 10;
            odom->setInformationMatrix(info);
            _mapManager->addRelation(odom);
            currentNode->setOdometry(odom);
            currentNode->setPreviousNode(_previousNode);
        }
        put(currentNode);
        if (odom)
            put(odom);
        _previousNode = currentNode;
        _previousNodeTransform = currentNodeTransform;
    }
}

void PointsKalmanFilterPredictor::predict(double time, const std::vector<Eigen::Vector3d> &u, std::vector<Eigen::Matrix<double, 6, 1> >& mu, std::vector<Eigen::Matrix<double, 6, 6> >& sigma)
{
    if (time == 0.)
    {
        mu = mus_;
        sigma = sigmas_;
    }
    else
    {
        Eigen::Matrix<double, 6, 6> A;
        Eigen::Matrix<double, 6, 3> B;

        A.setIdentity();
        A.block(0, 3, 3, 3) = Eigen::Matrix3d::Identity() * time;

        B.block(0, 0, 3, 3).setIdentity();
        B.block(3, 0, 3, 3) = Eigen::Matrix3d::Identity() * time;

        mu.resize(mus_.size());
        sigma.resize(sigmas_.size());

        for (int i=0; i<mu.size(); i++)
        {
            mu[i] = A * mus_[i] + B * u[i];
            sigma[i] = A * sigmas_[i] * A.transpose() + R_;
        }
    }
}

 UncertainTransformation::covariance_t UncertainTransformation::UOplus() const
 {
   Eigen::Matrix<double,6,6> S;
   S.setIdentity();
   S.topRightCorner<3,3>() = -sm::kinematics::crossMx(_t_a_b_a);
   return S.inverse().eval()*_U*S.transpose().inverse().eval();
 }

//-- private methods ----
static void MakeStateTransitionMatrix(
	const float dT,
	Eigen::Matrix<float, 6, 6> &m)
{
	m.setIdentity();
	m(0,3) = dT;
	m(1,4) = dT;
	m(2,5) = dT;
}

 inline
 Eigen::Matrix<T, R1, C1>
 mdivide_left_tri(const Eigen::Matrix<T, R1, C1> &A) {
   stan::math::check_square("mdivide_left_tri", "A", A);
   int n = A.rows();
   Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> b;
   b.setIdentity(n, n);
   A.template triangularView<TriView>().solveInPlace(b);
   return b;
 }

g2o::EdgeSE3 * Map3DbaseGraph::getG2OEdge(Transformation * transformation)
{
	Eigen::Affine3f eigenTransform(transformation->transformationMatrix);
	Eigen::Quaternionf eigenRotation(eigenTransform.rotation());
	g2o::SE3Quat transfoSE3(
			Eigen::Quaterniond(eigenRotation.w(), eigenRotation.x(), eigenRotation.y(), eigenRotation.z()),
			Eigen::Vector3d(eigenTransform(0,3), eigenTransform(1,3), eigenTransform(2,3)));
	g2o::EdgeSE3* edgeSE3 = new g2o::EdgeSE3;	
	edgeSE3->vertices()[0] = graphoptimizer.vertex(transformation->src->id);
	edgeSE3->vertices()[1] = graphoptimizer.vertex(transformation->dst->id);
	edgeSE3->setMeasurement(transfoSE3.inverse());
	edgeSE3->setInverseMeasurement(transfoSE3);
	Eigen::Matrix<double, 6, 6, 0, 6, 6> mat;
	mat.setIdentity(6,6);
	edgeSE3->information() = mat;
	return edgeSE3;
}

    static bool testApply()
    {
        bool b, ret;
        // apply delta:
        Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
        Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
        Eigen::Matrix<double, 4, 4> diff;

        SE3<double> pose;
        pose.set( delta );
        delta[ 0 ] = Math::deg2Rad( 1.5 );
        delta[ 1 ] = Math::deg2Rad( 1.1 );
        delta[ 2 ] = Math::deg2Rad( 1.6 );
        delta[ 3 ] = 1;
        delta[ 4 ] = 1;
        delta[ 5 ] = 1;
        pose.apply( delta );

        expectedT( 0, 3 ) = delta[ 3 ];
        expectedT( 1, 3 ) = delta[ 4 ];
        expectedT( 2, 3 ) = delta[ 5 ];

        Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
        double angle = axis.norm();	axis /= angle;

        expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
        diff = expectedT - pose.transformation();

        ret = b = ( diff.array().abs().sum() / 12 < 0.001 );

        if( !b ){
            std::cout << expectedT << std::endl;
            std::cout << pose.transformation() << std::endl;
            std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
        }

        pose.apply( -delta );
        expectedT.setIdentity();

        b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
        CVTTEST_PRINT( "apply", b );
        ret &= b;

        return ret;
    }

void check_is_diagonal()
{
    Eigen::Matrix<fl::Real, Size, Size> m;

    m.resize(Dim, Dim);
    m.setRandom();

    EXPECT_FALSE(fl::is_diagonal(m));

    m = m.diagonal().asDiagonal();
    EXPECT_TRUE(fl::is_diagonal(m));

    m.setIdentity();
    m *= 3.;
    EXPECT_TRUE(fl::is_diagonal(m));

    m(0, Dim - 1) = 2;
    EXPECT_FALSE(fl::is_diagonal(m));
}

int main(int, char**) {
  using SE3Type = Sophus::SE3<double>;
  using SO3Type = Sophus::SO3<double>;
  using Point = SE3Type::Point;
  double const kPi = Sophus::Constants<double>::pi();

  std::vector<SE3Type> se3_vec;
  se3_vec.push_back(
      SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)));
  se3_vec.push_back(
      SE3Type(SO3Type::exp(Point(0.2, 0.5, -1.0)), Point(10, 0, 0)));
  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.)), Point(0, 100, 5)));
  se3_vec.push_back(
      SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0, 0, 0)));
  se3_vec.push_back(SE3Type(SO3Type::exp(Point(0., 0., 0.00001)),
                            Point(0, -0.00000001, 0.0000000001)));
  se3_vec.push_back(
      SE3Type(SO3Type::exp(Point(0., 0., 0.00001)), Point(0.01, 0, 0)));
  se3_vec.push_back(SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(4, -5, 0)));
  se3_vec.push_back(
      SE3Type(SO3Type::exp(Point(0.2, 0.5, 0.0)), Point(0, 0, 0)) *
      SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
      SE3Type(SO3Type::exp(Point(-0.2, -0.5, -0.0)), Point(0, 0, 0)));
  se3_vec.push_back(
      SE3Type(SO3Type::exp(Point(0.3, 0.5, 0.1)), Point(2, 0, -7)) *
      SE3Type(SO3Type::exp(Point(kPi, 0, 0)), Point(0, 0, 0)) *
      SE3Type(SO3Type::exp(Point(-0.3, -0.5, -0.1)), Point(0, 6, 0)));

  for (size_t i = 0; i < se3_vec.size(); ++i) {
    bool const passed = test(se3_vec[i], se3_vec[(i + 3) % se3_vec.size()]);
    if (!passed) {
      std::cerr << "failed!" << std::endl << std::endl;
      exit(-1);
    }
  }

  Eigen::Matrix<ceres::Jet<double, 28>, 4, 4> mat;
  mat.setIdentity();
  std::cout << CreateSE3FromMatrix(mat) << std::endl;

  return 0;
}

pcl::simulation::TexturedQuad::TexturedQuad (int width, int height) : width_ (width), height_ (height)
{
  program_ = gllib::Program::loadProgramFromFile ("single_texture.vert", "single_texture.frag");
  program_->use ();
  Eigen::Matrix<float, 4, 4> MVP;
  MVP.setIdentity();
  program_->setUniform ("MVP", MVP);
  glUseProgram (0);

  glGenTextures (1, &texture_);
  glBindTexture (GL_TEXTURE_2D, texture_);
  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_MODE, GL_NONE);
  glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_COMPARE_FUNC, GL_LEQUAL);
  glTexImage2D (GL_TEXTURE_2D, 0, GL_RGB, width_, height_, 0, GL_RGB, GL_UNSIGNED_BYTE, NULL);
  glBindTexture (GL_TEXTURE_2D, 0);
}

inline Eigen::Affine3f
pcl::TransformationFromCorrespondences::getTransformation ()
{
  //Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
  Eigen::JacobiSVD<Eigen::Matrix<float, 3, 3> > svd (covariance_, Eigen::ComputeFullU | Eigen::ComputeFullV);
  const Eigen::Matrix<float, 3, 3>& u = svd.matrixU(),
                                   & v = svd.matrixV();
  Eigen::Matrix<float, 3, 3> s;
  s.setIdentity();
  if (u.determinant()*v.determinant() < 0.0f)
    s(2,2) = -1.0f;
  
  Eigen::Matrix<float, 3, 3> r = u * s * v.transpose();
  Eigen::Vector3f t = mean2_ - r*mean1_;
  
  Eigen::Affine3f ret;
  ret(0,0)=r(0,0); ret(0,1)=r(0,1); ret(0,2)=r(0,2); ret(0,3)=t(0);
  ret(1,0)=r(1,0); ret(1,1)=r(1,1); ret(1,2)=r(1,2); ret(1,3)=t(1);
  ret(2,0)=r(2,0); ret(2,1)=r(2,1); ret(2,2)=r(2,2); ret(2,3)=t(2);
  ret(3,0)=0.0f;   ret(3,1)=0.0f;   ret(3,2)=0.0f;   ret(3,3)=1.0f;
  
  return (ret);
}

  void setGains(const hrl_pr2_force_ctrl::HybridCartesianGains::ConstPtr &msg) // khawkins
  {
    
    // Store gains...
    if (msg->p_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kp[i] = msg->p_gains[i];
    if (msg->d_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kd[i] = msg->d_gains[i];

    // Store force gains... khawkins
    if (msg->fp_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kfp[i] = msg->fp_gains[i];
    if (msg->fi_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kfi[i] = msg->fi_gains[i];
    if (msg->fi_max_gains.size() == 6)
      for (size_t i = 0; i < 6; ++i)
        Kfi_max[i] = msg->fi_max_gains[i];

    // Store selector matricies khawkins
    if (msg->force_selector.size() == 6)
        for (size_t i = 0; i < msg->force_selector.size(); ++i)
            if(msg->force_selector[i]) {
                force_selector[i] = 1;
                motion_selector[i] = 0;
            } else {
                force_selector[i] = 0;
                motion_selector[i] = 1;
            }

    // Store frame information
    if(!msg->header.frame_id.compare(root_name_))
    {
      use_tip_frame_ = false;
      ROS_INFO("New gains in root frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
               root_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
      St.setIdentity();
    }
    else if(!msg->header.frame_id.compare(tip_name_))
    {
      use_tip_frame_ = true;
      ROS_INFO("New gains in tip frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
               tip_name_.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
      
    }
    else
    {
      use_tip_frame_ = false;
      
      geometry_msgs::PoseStamped in_root;
      in_root.pose.orientation.w = 1.0;
      in_root.header.frame_id = msg->header.frame_id;

      try {
        tf_.waitForTransform(root_name_, msg->header.frame_id, msg->header.stamp, ros::Duration(0.1));
        tf_.transformPose(root_name_, in_root, in_root);
      }
      catch (const tf::TransformException &ex)
      {
        ROS_ERROR("Failed to transform: %s", ex.what());
        return;
      }
      
      Eigen::Affine3d t;
      
      tf::poseMsgToEigen(in_root.pose, t);

      St << 
          t(0,0),t(0,1),t(0,2),0,0,0,
          t(1,0),t(1,1),t(1,2),0,0,0,
          t(2,0),t(2,1),t(2,2),0,0,0,
          0,0,0,t(0,0),t(0,1),t(0,2),
          0,0,0,t(1,0),t(1,1),t(1,2),
          0,0,0,t(2,0),t(2,1),t(2,2);
    
      St.transposeInPlace();
  
      ROS_INFO("New gains in arbitrary frame [%s]: %.1lf, %.1lf, %.1lf %.1lf, %.1lf, %.1lf",
             msg->header.frame_id.c_str(), Kp[0], Kp[1], Kp[2], Kp[3], Kp[4], Kp[5]);
    }
  }

 inline void inverse(Eigen::Matrix<double,R,C> &invA) const {
   invA.setIdentity(N_);
   _ldltP->solveInPlace(invA);
 }

bool HybridForceController::init(pr2_mechanism_model::RobotState *robot_state, ros::NodeHandle &n)
{
  rosrt::init();
  node_ = n;

  ROS_INFO_STREAM("JTTask controller compiled at " << __TIME__ );
  // get name of root and tip from the parameter server
  // std::string tip_name; // aleeper: Should use the class member instead!
  if (!node_.getParam("root_name", root_name_)){
    ROS_ERROR("HybridForceController: No root name found on parameter server (namespace: %s)",
              node_.getNamespace().c_str());
    return false;
  }
  if (!node_.getParam("tip_name", tip_name_)){
    ROS_ERROR("HybridForceController: No tip name found on parameter server (namespace: %s)",
              node_.getNamespace().c_str());
    return false;
  }

  // test if we got robot pointer
  assert(robot_state);
  robot_state_ = robot_state;

  // Chain of joints
  if (!chain_.init(robot_state_, root_name_, tip_name_))
    return false;
  if (!chain_.allCalibrated())
  {
    ROS_ERROR("Not all joints in the chain are calibrated (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }


  // Kinematics
  KDL::Chain kdl_chain;
  chain_.toKDL(kdl_chain);
  kin_.reset(new Kin<Joints>(kdl_chain));

  // Cartesian gains
  double kp_trans, kd_trans, kp_rot, kd_rot;
  if (!node_.getParam("cart_gains/trans/p", kp_trans) ||
      !node_.getParam("cart_gains/trans/d", kd_trans))
  {
    ROS_ERROR("P and D translational gains not specified (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  if (!node_.getParam("cart_gains/rot/p", kp_rot) ||
      !node_.getParam("cart_gains/rot/d", kd_rot))
  {
    ROS_ERROR("P and D rotational gains not specified (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  Kp << kp_trans, kp_trans, kp_trans,  kp_rot, kp_rot, kp_rot;
  Kd << kd_trans, kd_trans, kd_trans,  kd_rot, kd_rot, kd_rot;

  // aleeper
  use_tip_frame_ = false;
  if (!node_.getParam("use_tip_frame", use_tip_frame_)){
    ROS_WARN("HybridForceController: use_tip_frame was not specified, assuming 'false': %s)",
              node_.getNamespace().c_str());
  }
  St.setIdentity();

  // Force desired khawkins
  F_des_.setZero();
  F_integ_.setZero();
  K_effective_.setZero();

  double f_trans_max, f_rot_max;
  node_.param("force_trans_max", f_trans_max, 9999.0);
  node_.param("force_rot_max", f_rot_max, 9999.0);
  F_max_ << f_trans_max, f_trans_max, f_trans_max,  f_rot_max, f_rot_max, f_rot_max;

  // Force gains khawkins
  double kfp_trans, kfp_rot, kfi_trans, kfi_rot, kfi_max_trans, kfi_max_rot;
  if (!node_.getParam("force_gains/trans/p", kfp_trans) ||
      !node_.getParam("force_gains/trans/i", kfi_trans) ||
      !node_.getParam("force_gains/trans/i_max", kfi_max_trans))
  {
    ROS_ERROR("P and I translational force gains not specified (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  if (!node_.getParam("force_gains/rot/p", kfp_rot) ||
      !node_.getParam("force_gains/rot/i", kfi_rot) ||
      !node_.getParam("force_gains/rot/i_max", kfi_max_rot))
  {
    ROS_ERROR("P and I rotational force gains not specified (namespace: %s)", node_.getNamespace().c_str());
    return false;
  }
  Kfp << kfp_trans, kfp_trans, kfp_trans,  kfp_rot, kfp_rot, kfp_rot;
  Kfi << kfi_trans, kfi_trans, kfi_trans,  kfi_rot, kfi_rot, kfi_rot;
  Kfi_max << kfi_max_trans, kfi_max_trans, kfi_max_trans,  kfi_max_rot, kfi_max_rot, kfi_max_rot;
  motion_selector = CartVec::Ones();
  force_selector = CartVec::Zero(); 
  force_filter_.resize(6);
  for(int i=0;i<6;i++) {
    force_filter_[i].reset(new filters::FilterChain<double>("double"));
    force_filter_[i]->configure("force_filter", node_);
    ros::Duration(0.2).sleep();
  }
//.........这里部分代码省略.........