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

void mesh_core::PlaneProjection::getFrame(Eigen::Affine3d& frame) const
{
  frame.setIdentity();
  frame.translation() = origin_;
  frame.linear().col(0) = x_axis_;
  frame.linear().col(1) = y_axis_;
  frame.linear().col(2) = normal_;
}

// The input 3D points are stored as columns.
Eigen::Affine3d Find3DAffineTransform(Eigen::Matrix3Xd in, Eigen::Matrix3Xd out) {

  // Default output
  Eigen::Affine3d A;
  A.linear() = Eigen::Matrix3d::Identity(3, 3);
  A.translation() = Eigen::Vector3d::Zero();

  if (in.cols() != out.cols())
    throw "Find3DAffineTransform(): input data mis-match";

  // First find the scale, by finding the ratio of sums of some distances,
  // then bring the datasets to the same scale.
  double dist_in = 0, dist_out = 0;
  for (int col = 0; col < in.cols()-1; col++) {
    dist_in  += (in.col(col+1) - in.col(col)).norm();
    dist_out += (out.col(col+1) - out.col(col)).norm();
  }
  if (dist_in <= 0 || dist_out <= 0)
    return A;
  double scale = dist_out/dist_in;
  out /= scale;

  // Find the centroids then shift to the origin
  Eigen::Vector3d in_ctr = Eigen::Vector3d::Zero();
  Eigen::Vector3d out_ctr = Eigen::Vector3d::Zero();
  for (int col = 0; col < in.cols(); col++) {
    in_ctr  += in.col(col);
    out_ctr += out.col(col);
  }
  in_ctr /= in.cols();
  out_ctr /= out.cols();
  for (int col = 0; col < in.cols(); col++) {
    in.col(col)  -= in_ctr;
    out.col(col) -= out_ctr;
  }

  // SVD
  Eigen::MatrixXd Cov = in * out.transpose();
  Eigen::JacobiSVD<Eigen::MatrixXd> svd(Cov, Eigen::ComputeThinU | Eigen::ComputeThinV);

  // Find the rotation
  double d = (svd.matrixV() * svd.matrixU().transpose()).determinant();
  if (d > 0)
    d = 1.0;
  else
    d = -1.0;
  Eigen::Matrix3d I = Eigen::Matrix3d::Identity(3, 3);
  I(2, 2) = d;
  Eigen::Matrix3d R = svd.matrixV() * I * svd.matrixU().transpose();

  // The final transform
  A.linear() = scale * R;
  A.translation() = scale*(out_ctr - R*in_ctr);

  return A;
}

void TestFind3DAffineTransform(){

  // Create datasets with known transform
  Eigen::Matrix3Xd in(3, 100), out(3, 100);
  Eigen::Quaternion<double> Q(1, 3, 5, 2);
  Q.normalize();
  Eigen::Matrix3d R = Q.toRotationMatrix();
  double scale = 2.0;
  for (int row = 0; row < in.rows(); row++) {
    for (int col = 0; col < in.cols(); col++) {
      in(row, col) = log(2*row + 10.0)/sqrt(1.0*col + 4.0) + sqrt(col*1.0)/(row + 1.0);
    }
  }
  Eigen::Vector3d S;
  S << -5, 6, -27;
  for (int col = 0; col < in.cols(); col++)
    out.col(col) = scale*R*in.col(col) + S;

  Eigen::Affine3d A = Find3DAffineTransform(in, out);

  // See if we got the transform we expected
  if ( (scale*R-A.linear()).cwiseAbs().maxCoeff() > 1e-13 ||
       (S-A.translation()).cwiseAbs().maxCoeff() > 1e-13)
    throw "Could not determine the affine transform accurately enough";
}

void myCallback1(const interaction_cursor_msgs::InteractionCursorUpdate poseSubscribed) {
    // std::cout<<" i am in mycallback "<<std::endl;

    target1 = Eigen::Affine3d::Identity();



    Eigen::Quaterniond q(poseSubscribed.pose.pose.orientation.w,
            poseSubscribed.pose.pose.orientation.x,
            poseSubscribed.pose.pose.orientation.y,
            poseSubscribed.pose.pose.orientation.z);

    translation1 << poseSubscribed.pose.pose.position.x,
            poseSubscribed.pose.pose.position.y,
            poseSubscribed.pose.pose.position.z;
    std::cout << " 3 " << std::endl;
    rotation1 = q.toRotationMatrix();
    target1.translation() = translation1;
    target1.linear() = rotation1;




    std::cout << "please move hydra to a proper place and click the button. " << std::endl;

    if (poseSubscribed.button_state == 1) {
        button1 = true;

    }
}

void 
RGBID_SLAM::VisualizationManager::getPointCloud(pcl::PointCloud<pcl::PointXYZRGB>::Ptr whole_point_cloud)
{  
  IdToPoseMap::iterator id2pose_map_it;        
  IdToPointCloudMap::const_iterator id2pointcloud_map_it;
  
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr aux_point_cloud;  
  aux_point_cloud.reset(new pcl::PointCloud<pcl::PointXYZRGB>);
  
  for (id2pose_map_it = id2pose_map_.begin(); 
       id2pose_map_it != id2pose_map_.end();
       id2pose_map_it++)
  {
    int kf_id = (*id2pose_map_it).first;
    Eigen::Affine3d pose = (*id2pose_map_it).second;
    
    id2pointcloud_map_it = id2pointcloud_map_.find(kf_id);
    
    if (id2pointcloud_map_it != id2pointcloud_map_.end())
    {
      aux_point_cloud->clear();
      copyPointCloud((*id2pointcloud_map_it).second, aux_point_cloud);  
      (*id2pointcloud_map_it).second->clear();
      alignPointCloud(aux_point_cloud, pose.linear(), pose.translation());   
      *whole_point_cloud += *aux_point_cloud;
    }          
  }  
  
  
}

  void commandForce(const geometry_msgs::WrenchStamped::ConstPtr &msg)
  {
    F_des_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, 
              msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z;
    if(msg->header.frame_id == root_name_)
      return;

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

    F_des_.head<3>() = t.linear() * F_des_.head<3>();
    F_des_.tail<3>() = t.linear() * F_des_.tail<3>();
  }

/**
 * @function KState
 */
Eigen::Affine3d KState::xform() const {

  Eigen::Affine3d xform = Eigen::Affine3d::Identity();
  xform.linear() = body_rot.toRotationMatrix();
  xform.translation() = body_pos;
  
  return xform;
}

void RockinPNPActionServer::tf2Affine(tf::StampedTransform& tf, Eigen::Affine3d& T)
{
  tf::Vector3 o=tf.getOrigin();
  tf::Quaternion q_tf=tf.getRotation();
  Eigen::Quaterniond q(q_tf[3],q_tf[0],q_tf[1],q_tf[2]);
  Eigen::Matrix3d R(q);
  Eigen::Vector3d t(o[0],o[1],o[2]);
  T.linear()=R; T.translation()=t;
  
}

int ShapeGraspPlanner::createGrasp(const geometry_msgs::PoseStamped& pose,
                                   double gripper_opening,
                                   double gripper_pitch,
                                   double x_offset,
                                   double z_offset,
                                   double quality)
{
  moveit_msgs::Grasp grasp;
  grasp.grasp_pose = pose;

  // defaults
  grasp.pre_grasp_posture = makeGraspPosture(gripper_opening);
  grasp.grasp_posture = makeGraspPosture(0.0);
  grasp.pre_grasp_approach = makeGripperTranslation(approach_frame_,
                                                    approach_min_translation_,
                                                    approach_desired_translation_);
  grasp.post_grasp_retreat = makeGripperTranslation(retreat_frame_,
                                                    retreat_min_translation_,
                                                    retreat_desired_translation_,
                                                    -1.0);  // retreat is in negative x direction

  // initial pose
  Eigen::Affine3d p = Eigen::Translation3d(pose.pose.position.x,
                                           pose.pose.position.y,
                                           pose.pose.position.z) *
                        Eigen::Quaterniond(pose.pose.orientation.w,
                                           pose.pose.orientation.x,
                                           pose.pose.orientation.y,
                                           pose.pose.orientation.z);
  // translate by x_offset, 0, z_offset
  p = p * Eigen::Translation3d(x_offset, 0, z_offset);
  // rotate by 0, pitch, 0
  p = p * quaternionFromEuler(0.0, gripper_pitch, 0.0);
  // apply grasp point -> planning frame offset
  p = p * Eigen::Translation3d(-tool_offset_, 0, 0);

  grasp.grasp_pose.pose.position.x = p.translation().x();
  grasp.grasp_pose.pose.position.y = p.translation().y();
  grasp.grasp_pose.pose.position.z = p.translation().z();
  Eigen::Quaterniond q = (Eigen::Quaterniond)p.linear();
  grasp.grasp_pose.pose.orientation.x = q.x();
  grasp.grasp_pose.pose.orientation.y = q.y();
  grasp.grasp_pose.pose.orientation.z = q.z();
  grasp.grasp_pose.pose.orientation.w = q.w();

  grasp.grasp_quality = quality;

  grasps_.push_back(grasp);
  return 1;
}

bool
pcl::EnsensoGrabber::jsonTransformationToMatrix (const std::string transformation,
                                                 Eigen::Affine3d &matrix) const
{
  try
  {
    NxLibCommand convert_transformation (cmdConvertTransformation);
    convert_transformation.parameters ()[itmTransformation].setJson (transformation);
    convert_transformation.execute ();
    Eigen::Affine3d tmp (Eigen::Affine3d::Identity ());

    // Rotation
    tmp.linear ().col (0) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][0][0].asDouble (),
                                             convert_transformation.result ()[itmTransformation][0][1].asDouble (),
                                             convert_transformation.result ()[itmTransformation][0][2].asDouble ());

    tmp.linear ().col (1) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][1][0].asDouble (),
                                             convert_transformation.result ()[itmTransformation][1][1].asDouble (),
                                             convert_transformation.result ()[itmTransformation][1][2].asDouble ());

    tmp.linear ().col (2) = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][2][0].asDouble (),
                                             convert_transformation.result ()[itmTransformation][2][1].asDouble (),
                                             convert_transformation.result ()[itmTransformation][2][2].asDouble ());

    // Translation
    tmp.translation () = Eigen::Vector3d (convert_transformation.result ()[itmTransformation][3][0].asDouble (),
                                          convert_transformation.result ()[itmTransformation][3][1].asDouble (),
                                          convert_transformation.result ()[itmTransformation][3][2].asDouble ());
    matrix = tmp;
    return (true);
  }
  catch (NxLibException &ex)
  {
    ensensoExceptionHandling (ex, "jsonTransformationToMatrix");
    return (false);
  }
}

//utility to convert a pose to an Eigen::Affine
Eigen::Affine3d transformPoseToEigenAffine3d(geometry_msgs::Pose pose) {
    
    Eigen::Affine3d e;
    Eigen::Vector3d Oe;
    Oe(0)=pose.position.x;
    Oe(1)=pose.position.y;
    Oe(2)=pose.position.z;

    Eigen::Quaterniond q;
    q.x()=pose.orientation.x;
    q.y()=pose.orientation.y;
    q.z()=pose.orientation.z;
    q.w()=pose.orientation.w;  
    Eigen::Matrix3d Re(q);
    e.linear()=Re;
    e.translation() = Oe;
    return e;
}

bool ObjectManipulationProperties::get_object_info(int object_id, Eigen::Affine3d &grasp_transform, double &approach_dist, double &gripper_close_test) {
    bool got_info = false;
    Eigen::Vector3d object_origin_wrt_gripper_frame;
    //Eigen::Quaternion object_orientation_wrt_gripper_frame;
    Eigen::Matrix3d R_object_wrt_gripper;
    Eigen::Matrix3d R_gripper;
    Eigen::Vector3d x_axis, y_axis, z_axis;
   Eigen::Vector3d origin_object_wrt_gripper;    
    switch (object_id) {
        case TOY_BLOCK_ID:
            //set approach distance and gripper-closure test val: MAGIC NUMBERS
            // appropriate ONLY for this object with Baxter right-hand gripper in simu
            approach_dist = 0.05; //old float64 TOY_BLOCK_APPROACH_DIST = 0.05
            gripper_close_test = 83.0; //old float64 TOY_BLOCK_GRIPPER_CLOSE_TEST_VAL = 80.0
            //derive gripper approach pose from block pose:
            //compute a gripper pose with z-axis anti-parallel to object z-axis,
            // and x-axis coincident with object x-axis

            origin_object_wrt_gripper<<0,0,0;
            x_axis<<1,0,0; // make block x-axis parallel to gripper x-axis;
            z_axis<<0,0,-1; // gripper z axis antiparallel to block z axis;
            y_axis = z_axis.cross(x_axis); //consistent triad

            R_gripper.col(0) = x_axis; //populate orientation matrix from axis directions
            R_gripper.col(1) = y_axis;
            R_gripper.col(2) = z_axis;
            //gripper_affine is defined to have origin coincident w/ block-frame origin, but z-axis antiparallel to block z-axis
            // and x-axis parallel to block-frame x-axis
            grasp_transform.linear() = R_gripper; //populate affine w/ orientation
            grasp_transform.translation() = origin_object_wrt_gripper; //and origin          

            return true;
            break;

            //case SOMETHING_ELSE:  //add more object cases here!
            //...
            //return true;
            //break;
        default:
            ROS_WARN("object ID not recognized");
            return false;
            break;
    }
}

geometry_msgs::Pose MotionPlanning::transformEigenAffine3dToPose(Eigen::Affine3d e) {
    Eigen::Vector3d Oe;
    Eigen::Matrix3d Re;
    geometry_msgs::Pose pose;
    Oe = e.translation();
    Re = e.linear();

    Eigen::Quaterniond q(Re); // convert rotation matrix Re to a quaternion, q
    pose.position.x = Oe(0);
    pose.position.y = Oe(1);
    pose.position.z = Oe(2);

    pose.orientation.x = q.x();
    pose.orientation.y = q.y();
    pose.orientation.z = q.z();
    pose.orientation.w = q.w();

    return pose;
}

bool ArmMotionInterface::unpack_goal_pose(void) {
    geometry_msgs::PoseStamped poseStamped_goal = request_.poseStamped_goal;
    Eigen::Vector3d goal_origin;
    goal_origin[0] = poseStamped_goal.pose.position.x;
    goal_origin[1] = poseStamped_goal.pose.position.y;
    goal_origin[2] = poseStamped_goal.pose.position.z;
    a_tool_end_.translation() = goal_origin;

    Eigen::Quaterniond quat;
    quat.x() = poseStamped_goal.pose.orientation.x;
    quat.y() = poseStamped_goal.pose.orientation.y;
    quat.z() = poseStamped_goal.pose.orientation.z;
    quat.w() = poseStamped_goal.pose.orientation.w;
    Eigen::Matrix3d R_goal(quat);
    a_tool_end_.linear() = R_goal;
    a_flange_end_ = a_tool_end_ * A_tool_wrt_flange_.inverse();


    return true;
}

int Irb120_IK_solver::ik_solve(Eigen::Affine3d const& desired_hand_pose) {
    q6dof_solns.clear();
    bool reachable = compute_q123_solns(desired_hand_pose, q6dof_solns);
    if (!reachable) {
        return 0;
    }
    reachable = false;
    //is at least one solution within joint range limits?
    q_solns_fit.clear();
    Vectorq6x1 q_soln;
    Eigen::Matrix3d R_des;
    R_des = desired_hand_pose.linear();
    int nsolns = q6dof_solns.size();
    bool fits;

    std::vector<Vectorq6x1> q_wrist_solns;
    for (int i=0;i<nsolns;i++) {
        q_soln = q6dof_solns[i];
        fits = fit_joints_to_range(q_soln); // force q_soln in to periodic range, if possible, and return if possible
        if (fits) { // if here, then have a valid 3dof soln; try to get wrist solutions
            // get wrist solutions; expect 2, though not checked for joint limits
            solve_spherical_wrist(q_soln,R_des, q_wrist_solns);  
            int n_wrist_solns = q_wrist_solns.size();
            for (int iwrist=0;iwrist<n_wrist_solns;iwrist++) {
                q_soln = q_wrist_solns[iwrist];
                if (fit_joints_to_range(q_soln)) {
                  q_solns_fit.push_back(q_soln);
                  reachable = true; // note that we have at least one reachable solution
                }
            }

        }
    }
    if (!reachable) {
        return 0;
    }
    
    //if here, have reachable solutions
    nsolns = q_solns_fit.size();
    return nsolns;
}