C++ Matrix4d::block方法代码示例

 void calculateInverseTransformationMatrix(const Eigen::Matrix4d& src, Eigen::Matrix4d& dst)
 {
   dst = Eigen::Matrix4d::Identity();
   Eigen::Matrix3d R_trans = src.block(0, 0, 3, 3).transpose();
   dst.block(0, 0, 3, 3) = R_trans;
   dst.block(0, 3, 3, 1) = -R_trans * src.block(0, 3, 3, 1);
 }

void setTransform(Eigen::Matrix3d &rot, Eigen::Vector3d &t, Eigen::Matrix4d &Tf)
{

Tf.block(0,0,3,3) << rot;
Tf.block(0,3,3,1) << t;
Tf.block(3,0,1,4) << 0.0,0.0,0.0,1.0;

}

Eigen::Matrix4d recenter_transform3d(const Eigen::Matrix4d &transfo, const Eigen::Vector3d& center)
{
	//% remove former translation part
	Eigen::Matrix4d res = Eigen::Matrix4d::Identity();
	res.block(0, 0, 3, 3) = transfo.block(0, 0, 3, 3);

	//% create translations
	Eigen::Matrix4d t1 = create_translation3d(-center);
	Eigen::Matrix4d t2 = create_translation3d(center);

	//% compute translated transform
	res = t2*res*t1;
	return res;
}

void Manipulator::computeCabs()
{
  if(C_abs_.size() != T_abs_.size())
  {
    std::stringstream msg;
    msg << "C_abs_.size() != T_abs_.size()" << std::endl
        << "  C_abs_.size : " << C_abs_.size() << std::endl
        << "  T_abs_.size : " << T_abs_.size();
    throw ahl_utils::Exception("Manipulator::computeCabs", msg.str());
  }
  else if(C_abs_.size() != link_.size())
  {
    std::stringstream msg;
    msg << "C_abs_.size() != link_.size()" << std::endl
        << "  C_abs_.size : " << C_abs_.size() << std::endl
        << "  link_.size   : " << link_.size();
    throw ahl_utils::Exception("Manipulator::computeCabs", msg.str());
  }
  else if(C_abs_.size() == 0)
  {
    std::stringstream msg;
    msg << "C_abs_.size() == 0" << std::endl
        << "  C_abs_.size    : " << C_abs_.size();
    throw ahl_utils::Exception("Manipulator::computeCabs", msg.str());
  }

  for(uint32_t i = 0; i < C_abs_.size(); ++i)
  {
    Eigen::Matrix4d Tlc = Eigen::Matrix4d::Identity();
    Tlc.block(0, 3, 3, 1) = link_[i]->C;
    C_abs_[i] = T_abs_[i] * Tlc;
  }
}

int GraphOptimizer_G2O::addVertex(Eigen::Matrix4d& vertexPose, int id, bool isFixed)
{

    g2o::Vector3d t(vertexPose(0,3),vertexPose(1,3),vertexPose(2,3));

    Eigen::Matrix3d rot = vertexPose.block(0,0,3,3);
    g2o::Quaterniond q(rot);
    q.normalize();

    // set up node
    g2o::VertexSE3 *vc = new g2o::VertexSE3();
    Eigen::Isometry3d cam; // camera pose
    cam = q;
    cam.translation() = t;

    vc->setEstimate(cam);
    vc->setId(id);      // vertex id

    //set pose fixed
    if (isFixed) {
        vc->setFixed(true);
    }

    // add to optimizer
    optimizer.addVertex(vc);
    vertexIdVec.push_back(id);

    return id;
}

geometry_msgs::Pose Sensors::robot2sensorTransformation(geometry_msgs::Pose pose)
{


    Eigen::Matrix4d robotPoseMat, robot2sensorMat, sensorPoseMat;
    //Robot matrix pose
    Eigen::Matrix3d R; Eigen::Vector3d T1(pose.position.x,pose.position.y,pose.position.z);
    tf::Quaternion qt(pose.orientation.x,pose.orientation.y,pose.orientation.z,pose.orientation.w);
    tf::Matrix3x3 R1(qt);
    tf::matrixTFToEigen(R1,R);
    robotPoseMat.setZero ();
    robotPoseMat.block (0, 0, 3, 3) = R;
    robotPoseMat.block (0, 3, 3, 1) = T1;
    robotPoseMat (3, 3) = 1;

    //transformation matrix
    qt = tf::createQuaternionFromRPY(sensorRPY[0],sensorRPY[1],sensorRPY[2]);
    tf::Matrix3x3 R2(qt);Eigen::Vector3d T2(sensorPose[0], sensorPose[1], sensorPose[2]);
    tf::matrixTFToEigen(R2,R);
    robot2sensorMat.setZero ();
    robot2sensorMat.block (0, 0, 3, 3) = R;
    robot2sensorMat.block (0, 3, 3, 1) = T2;
    robot2sensorMat (3, 3) = 1;

    //preform the transformation
    sensorPoseMat = robotPoseMat * robot2sensorMat;

    Eigen::Matrix4d sensor2sensorMat; //the frustum culling sensor needs this
    //the transofrmation is rotation by +90 around x axis of the sensor
    sensor2sensorMat << 1, 0, 0, 0,
                        0, 0,-1, 0,
                        0, 1, 0, 0,
                        0, 0, 0, 1;
    Eigen::Matrix4d newSensorPoseMat = sensorPoseMat * sensor2sensorMat;
    geometry_msgs::Pose p;
    Eigen::Vector3d T3;Eigen::Matrix3d Rd; tf::Matrix3x3 R3;
    Rd = newSensorPoseMat.block (0, 0, 3, 3);
    tf::matrixEigenToTF(Rd,R3);
    T3 = newSensorPoseMat.block (0, 3, 3, 1);
    p.position.x=T3[0];p.position.y=T3[1];p.position.z=T3[2];
    R3.getRotation(qt);
    p.orientation.x = qt.getX(); p.orientation.y = qt.getY();p.orientation.z = qt.getZ();p.orientation.w = qt.getW();

    return p;

}

 void xyzrpyToTransformationMatrix(const Eigen::Vector3d& xyz, const Eigen::Vector3d& rpy, Eigen::Matrix4d& T)
 {
   T = Eigen::Matrix4d::Identity();
   Eigen::Matrix3d R;
   rpyToRotationMatrix(rpy, R);
   T.block(0, 0, 3, 3) = R;
   T.block(0, 3, 3, 1) = xyz;
 }

Eigen::Matrix4d createHomogeneousTransformMatrix(tf::StampedTransform transform)
{
	tf::Point p = transform.getOrigin();
	tf::Quaternion q = transform.getRotation();
	tf::Matrix3x3 R1(q);
	Eigen::Matrix3d R2;
	tf::matrixTFToEigen(R1, R2);
	ROS_INFO_STREAM("R2:\n"<<R2);

	Eigen::Matrix4d T;
	T.block(0, 0, 3, 3) << R2;
	T.block(0, 3, 3, 1) << p.x(), p.y(), p.z();
	T.row(3) << 0, 0, 0, 1;
	return T;
}

/**
 * @function getPlaneTransform
 * @brief Assumes plane coefficients are of the form ax+by+cz+d=0, normalized
*/
Eigen::Matrix4d getPlaneTransform ( pcl::ModelCoefficients &coeffs,
				    double up_direction,
				    bool flatten_plane ) {
  Eigen::Matrix4d tf = Eigen::Matrix4d::Identity();
  if( coeffs.values.size() <= 3 ) {
    std::cout << "[ERROR] Coefficient size less than 3. I will output nonsense values"<<std::endl;
    return tf;
  }
  double a = coeffs.values[0];
  double b = coeffs.values[1];
  double c = coeffs.values[2];
  double d = coeffs.values[3];
  //asume plane coefficients are normalized
  Eigen::Vector3d position(-a*d, -b*d, -c*d);
  Eigen::Vector3d z(a, b, c);
  //if we are flattening the plane, make z just be (0,0,up_direction)
  if(flatten_plane) {
    z << 0, 0, up_direction;
  }
  else {
    //make sure z points "up" (the plane normal and the table must have a > 90 angle, hence the cosine must be negative)
    if ( z.dot( Eigen::Vector3d(0, 0, up_direction) ) > 0) {
      z = -1.0 * z;
      printf("Changing sign \n");
      coeffs.values[0]*= -1; coeffs.values[1]*= -1; coeffs.values[2]*= -1; coeffs.values[3]*= -1;
    }
  }
  //try to align the x axis with the x axis of the original frame
  //or the y axis if z and x are too close too each other
  Eigen::Vector3d x; x << 1, 0, 0;
  if ( fabs(z.dot(x)) > 1.0 - 1.0e-4) x = Eigen::Vector3d(0, 1, 0);
  Eigen::Vector3d y = z.cross(x); y.normalized();
  x = y.cross(z); x.normalized();
  Eigen::Matrix3d rotation;
  rotation.block(0,0,3,1) = x;
  rotation.block(0,1,3,1) = y;
  rotation.block(0,2,3,1) = z;
  Eigen::Quaterniond orientation( rotation );
  tf.block(0,0,3,3) = orientation.matrix();
  tf.block(0,3,3,1) = position;
  return tf;
}

  void
  calibrate_stereo(const observation_pts_v_t& left_points, const observation_pts_v_t& right_points,
                   const object_pts_v_t& object_points, const cv::Size& image_size, PinholeCameraModel& left,
                   PinholeCameraModel& right)
  {
    PinholeCameraModel camera;
    StereoCameraModel scam;
    std::vector<cv::Mat> rvecs, tvecs;
    cv::Mat K_left, K_right, D_right, D_left, R, T, E, F;
    int flags =cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_ASPECT_RATIO /*| cv::CALIB_ZERO_TANGENT_DIST|cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 |cv::CALIB_FIX_K3*/;
    cv::Size left_size(image_size), right_size(image_size);
    double left_rms = cv::calibrateCamera(object_points, left_points, left_size, K_left, D_left, rvecs, tvecs, flags);
    double right_rms = cv::calibrateCamera(object_points, right_points, right_size, K_right, D_right, rvecs, tvecs,
                                           flags);

    double stereo_rms = cv::stereoCalibrate(object_points, left_points, right_points, K_left, D_left, K_right, D_right,
                                            left_size, R, T, E, F,
                                            cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 500, 1e-6),
                                            cv::CALIB_FIX_INTRINSIC | flags);

    std::cout << "image size:" << image_size.width << ":" << image_size.height << std::endl;
    std::cout << "R:" << R << std::endl;
    std::cout << "T:" << T << std::endl;
    std::cout << "D:" << D_left << std::endl;
    std::cout << "left rms:" << left_rms << " right rms:" << right_rms << " stereo rms:" << stereo_rms << std::endl;
    left.setParams(left_size, K_left, D_left, cv::Mat_<double>::eye(3, 3), K_left);
    right.setParams(right_size, K_right, D_right, cv::Mat_<double>::eye(3, 3), K_right);
    left.writeCalibration("left.yml");
    right.writeCalibration("right.yml");
    Eigen::Matrix3d Re;
    Eigen::Vector3d Te;
    Eigen::Matrix4d Pe = Eigen::Matrix4d::Identity();
    cv2eigen(R, Re);
    cv2eigen(T.t(), Te);
    std::cout << Te << std::endl;
    Pe.block(0, 0, 3, 3) = Re;
    Pe.block(0, 3, 3, 1) = Te;
    Pose P;
    P.transform.matrix() = Pe;
    scam.setParams(left, right, P);
    scam.writeCalibration("stereo.yml");
  }

void Manipulator::computeJacobian(int32_t idx, Eigen::MatrixXd& J)
{
  J = Eigen::MatrixXd::Zero(6, dof_);

  if(idx < dof_) // Not required to consider end-effector
  {
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }
  }
  else // Required to consider the offset of end-effector
  {
    --idx;
    for(uint32_t i = 0; i <= idx; ++i)
    {
      if(link_[i]->ep) // joint_type is prismatic
      {
        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * Eigen::Vector3d::Zero();
      }
      else // joint_type is revolute
      {
        Eigen::Matrix4d Tib;
        math::calculateInverseTransformationMatrix(T_abs_[i], Tib);
        Eigen::Matrix4d Cin = Tib * C_abs_[idx];
        Eigen::Vector3d P = Cin.block(0, 3, 3, 1);

        J.block(0, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis().cross(P);
        J.block(3, i, 3, 1) = T_abs_[i].block(0, 0, 3, 3) * link_[i]->tf->axis();
      }
    }

    Eigen::MatrixXd J_Pne = Eigen::MatrixXd::Identity(6, 6);
    Eigen::Vector3d Pne;
    if(C_abs_.size() - 1 - 1 >= 0.0)
    {
      Pne = T_abs_[T_abs_.size() - 1].block(0, 3, 3, 1) - C_abs_[C_abs_.size() - 1 - 1].block(0, 3, 3, 1);
    }
    else
    {
      std::stringstream msg;
      msg << "C_abs_.size() <= 1" << std::endl
          << "Manipulator doesn't have enough links." << std::endl;
      throw ahl_utils::Exception("Manipulator::computeJacobian", msg.str());
    }

    Eigen::Matrix3d Pne_cross;
    Pne_cross <<           0.0,  Pne.coeff(2), -Pne.coeff(1),
                 -Pne.coeff(2),           0.0,  Pne.coeff(0),
                  Pne.coeff(1), -Pne.coeff(0),           0.0;
    J_Pne.block(0, 3, 3, 3) = Pne_cross;
    J = J_Pne * J;
  }
}

void RevoluteZ::transform(double q, const Eigen::Matrix4d& T_org, Eigen::Matrix4d& T)
{
  T = T_org;
  T.block(0, 0, 3, 3) = T_org.block(0, 0, 3, 3) * this->T(q).block(0, 0, 3, 3);
}