C++ Matrix4f::col方法代码示例

Eigen::Matrix4f scale(const Eigen::Matrix4f &m, const Eigen::Vector3f &v) {
    Eigen::Matrix4f Result;
    Result.col(0) = m.col(0).array() * v(0);
    Result.col(1) = m.col(1).array() * v(1);
    Result.col(2) = m.col(2).array() * v(2);
    Result.col(3) = m.col(3);
    return Result;
}

Eigen::Vector3f transformVector(Eigen::Vector3f vector_in, Eigen::Matrix4f transf){
    Eigen::Vector4f vector_in_4f(vector_in[0], vector_in[1], vector_in[2], 1);
    // Obtener rotación desde matriz de transformación
    Eigen::Matrix4f rot;
    rot = transf;
    rot.col(3) = Eigen::Vector4f(0, 0, 0, 1);
    Eigen::Vector4f vector_out_4f = rot*Eigen::Vector4f(vector_in[0], vector_in[1], vector_in[2], 1);
    Eigen::Vector3f vector_out (vector_out_4f[0], vector_out_4f[1], vector_out_4f[2]);
    return vector_out;
}

void transform(pcl::PointCloud<pcl::PointXYZ> &pc, Eigen::Quaternionf &R,  Eigen::Vector3f &t)
{
  Eigen::Matrix4f m = Eigen::Matrix4f::Identity();
  for(int i=0; i<3; i++)
    for(int j=0; j<3; j++)
      m(i,j) = R.toRotationMatrix()(i,j);
  m.col(3).head<3>() = t;

  pcl::transformPointCloud(pc,pc,m);
}

template <typename PointSource, typename PointTarget> void
pcl::GeneralizedIterativeClosestPoint<PointSource, PointTarget>::applyState(Eigen::Matrix4f &t, const Vector6d& x) const
{
  // !!! CAUTION Stanford GICP uses the Z Y X euler angles convention
  Eigen::Matrix3f R;
  R = Eigen::AngleAxisf (static_cast<float> (x[5]), Eigen::Vector3f::UnitZ ())
    * Eigen::AngleAxisf (static_cast<float> (x[4]), Eigen::Vector3f::UnitY ())
    * Eigen::AngleAxisf (static_cast<float> (x[3]), Eigen::Vector3f::UnitX ());
  t.topLeftCorner<3,3> ().matrix () = R * t.topLeftCorner<3,3> ().matrix ();
  Eigen::Vector4f T (static_cast<float> (x[0]), static_cast<float> (x[1]), static_cast<float> (x[2]), 0.0f);
  t.col (3) += T;
}

void DepthFrame::getRegisteredAndReferencedPointCloud(pcl::PointCloud<pcl::PointXYZ>& cloud)
{
    pcl::PointXYZ regReferencePoint = getRegisteredReferencePoint();
    
    // Build a translation matrix to the registered reference the cloud after its own registration
    Eigen::Matrix4f E = Eigen::Matrix4f::Identity();
    E.col(3) = regReferencePoint.getVector4fMap(); // set translation column
    
    // Apply registration first and then referenciation (right to left order in matrix product)
    pcl::PointCloud<pcl::PointXYZ>::Ptr pCloudTmp (new pcl::PointCloud<pcl::PointXYZ>);
    getPointCloud(*pCloudTmp);
    
    pcl::transformPointCloud(*pCloudTmp, cloud, E.inverse() * m_T);
}

void OBJCTXT<_DOF6>::findCorrespondences3(const OBJCTXT &ctxt, std::vector<SCOR> &cors,
                                          const DOF6 &tf) {
  map_cors_.clear();

  Eigen::Matrix4f M = tf.getTF4().inverse();
  Eigen::Vector3f t=M.col(3).head<3>();
  Eigen::Matrix3f R=M.topLeftCorner(3,3);

  const float thr = tf.getRotationVariance()+0.05f;

  for(size_t j=0; j<ctxt.objs_.size(); j++)
  {
    OBJECT obj = *ctxt.objs_[j];
    obj.transform(R,t,0,0);

    for(size_t i=0; i<objs_.size(); i++)
      if( (obj.getData().getBB().preassumption(objs_[i]->getData().getBB())>=std::cos(thr+objs_[i]->getData().getBB().ratio())) &&
          obj.intersectsBB(*objs_[i], tf.getRotationVariance(),tf.getTranslationVariance())
          && obj.getData().extensionMatch(objs_[i]->getData(),0.7f,0)
      )
      {
        if(obj.intersectsPts(*objs_[i], tf.getRotationVariance(),tf.getTranslationVariance()) ||
            objs_[i]->intersectsPts(obj, tf.getRotationVariance(),tf.getTranslationVariance()))
        {
          //seccond check
          map_cors_[ctxt.objs_[j]].push_back(cors.size());
          SCOR c;
          c.a = objs_[i];
          c.b = ctxt.objs_[j];
          cors.push_back(c);
        }
      }

  }

#ifdef DEBUG_
  ROS_INFO("found %d correspondences (%d %d)", (int)cors.size(), (int)objs_.size(), (int)ctxt.objs_.size());

  for(typename std::vector<SCOR>::iterator it = cors.begin(); it!=cors.end(); it++)
  {
    Debug::Interface::get().addArrow(it->a->getNearestPoint(),it->b->getNearestPoint(), 0,255,0);
  }
#endif

}

geometry_msgs::Pose transformPose(geometry_msgs::Pose pose_in, Eigen::Matrix4f transf){
    geometry_msgs::Pose pose_out;
    // Obtener rotación desde matriz de transformación
    Eigen::Matrix4f rot;
    rot = transf;
    rot.col(3) = Eigen::Vector4f(0, 0, 0, 1);
    // Crear normal desde quaternion
    tf::Quaternion tf_q;
    tf::quaternionMsgToTF(pose_in.orientation, tf_q);
    tf::Vector3 normal(1, 0, 0);
    normal = tf::quatRotate(tf_q, normal);
    normal.normalize();
    // Rotar normal
    Eigen::Vector3f normal_vector (normal.x(), normal.y(), normal.z());
    Eigen::Vector3f normal_rotated = transformVector(normal_vector, transf);
    normal_rotated.normalize();
    // Obtener quaternion desde normal rotada
    pose_out.orientation = coefsToQuaternionMsg(normal_rotated[0], normal_rotated[1], normal_rotated[2]);
    // Transportar posición
    pose_out.position = transformPoint(pose_in.position, transf);
    return pose_out;
}

Eigen::Matrix4f getTransformation(string frame_ini, string frame_end){
    tf::TransformListener tf_listener;
    tf::StampedTransform stamped_tf;
    ros::Time transform_time = ros::Time(0);
    Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
    if (frame_ini.compare(frame_end) == 0)
        return transformation;
    try{
        ROS_INFO("UTIL: Esperando transformacion disponible...");
        if (not tf_listener.waitForTransform(frame_end, frame_ini, transform_time, ros::Duration(1))){
            ROS_ERROR("UTIL: Transformacion no pudo ser obtenida antes del timeout (%fs)", 1.0);
            return transformation;
        }
        ROS_INFO("UTIL: Guardando transformacion");
        tf_listener.lookupTransform(frame_end, frame_ini, ros::Time(0), stamped_tf);
    }
    catch (tf::TransformException ex){
        ROS_ERROR("UTIL: Excepcion al obtener transformacion: %s", ex.what());
        return transformation;
    }
    // tf::Matrix3x3 rotation = stamped_tf.getBasis();
    tf::Matrix3x3 rotation (stamped_tf.getRotation());
    tf::Vector3 translation = stamped_tf.getOrigin();
    Eigen::Matrix3f upleft3x3;
    upleft3x3 << rotation[0][0], rotation[0][1], rotation[0][2],
                 rotation[1][0], rotation[1][1], rotation[1][2],
                 rotation[2][0], rotation[2][1], rotation[2][2];
    // cout << "Rotation: \n" << upleft3x3 << endl;
    // cout << "Translation: \n" << translation[0] << "    \t" << translation[1] << "    \t" << translation[2] << endl; 
    transformation.block<3,3>(0,0) = upleft3x3;
    transformation.col(3) = Eigen::Vector4f(translation[0], translation[1], translation[2], 1);
    // cout << "Todo: \n" << transformation << endl;
    return transformation;
    /*  transformation << rotation[0][0], rotation[0][1], rotation[0][2], translation[0],
        rotation[1][0], rotation[1][1], rotation[1][2], translation[1],
        rotation[2][0], rotation[2][1], rotation[2][2], translation[2],
        0, 0, 0, 1;*/
}

Eigen::Matrix4f translate(const Eigen::Matrix4f &m, const Eigen::Vector3f &v) {
    Eigen::Matrix4f Result = m;
    Result.col(3) = m.col(0).array() * v(0) + m.col(1).array() * v(1) +
                    m.col(2).array() * v(2) + m.col(3).array();
    return Result;
}

    void trackNewCloud(const sensor_msgs::PointCloud2Ptr& msg)
    {
        ros::Time start_time_stamp = msg->header.stamp;

        boost::posix_time::ptime start_time = boost::posix_time::microsec_clock::local_time ();
        //std::cout << (start_time - last_cloud_).total_nanoseconds () * 1.0e-9 << std::endl;

        float time_ms = (start_time_stamp - last_cloud_ros_time_).toSec() * 1e3;

        last_cloud_ = start_time;
        last_cloud_ros_time_ = start_time_stamp;

        pcl::ScopeTime t("trackNewCloud");
        scene_.reset(new pcl::PointCloud<PointT>);
        pcl::moveFromROSMsg (*msg, *scene_);

        v4r::DataMatrix2D<Eigen::Vector3f> kp_cloud;
        cv::Mat_<cv::Vec3b> image;

        v4r::convertCloud(*scene_, kp_cloud, image);

        int cam_idx=-1;

        bool is_ok = camtracker->track(image, kp_cloud, pose_, conf_, cam_idx);

        if(debug_image_publisher_.getNumSubscribers())
        {
            drawConfidenceBar(image, conf_);
            sensor_msgs::ImagePtr image_msg = cv_bridge::CvImage(msg->header, "bgr8", image).toImageMsg();
            debug_image_publisher_.publish(image_msg);
        }

        std::cout << time_ms << " conf:" << conf_ << std::endl;

        if(is_ok)
        {
            selectFrames(*scene_, cam_idx, pose_);
            tf::Transform transform;

            //v4r::invPose(pose, inv_pose);
            transform.setOrigin(tf::Vector3(pose_(0,3), pose_(1,3), pose_(2,3)));
            tf::Matrix3x3 R(pose_(0,0), pose_(0,1), pose_(0,2),
                            pose_(1,0), pose_(1,1), pose_(1,2),
                            pose_(2,0), pose_(2,1), pose_(2,2));
            tf::Quaternion q;
            R.getRotation(q);
            transform.setRotation(q);
            ros::Time now_sync = ros::Time::now();
            cameraTransformBroadcaster.sendTransform(tf::StampedTransform(transform, now_sync, "camera_rgb_optical_frame", "world"));

            bool publish_trajectory = true;
            if (!trajectory_marker_.points.empty())
            {
                const geometry_msgs::Point& last = trajectory_marker_.points.back();
                Eigen::Vector3f v_last(last.x, last.y, last.z);
                Eigen::Vector3f v_curr(-pose_.col(3).head<3>());
                if ((v_last - v_curr).norm() < trajectory_threshold_)
                    publish_trajectory = false;
            }

            if (publish_trajectory)
            {
                geometry_msgs::Point p;
                p.x = -pose_(0,3);
                p.y = -pose_(1,3);
                p.z = -pose_(2,3);
                std_msgs::ColorRGBA c;
                c.a = 1.0;
                c.g = conf_;
                trajectory_marker_.points.push_back(p);
                trajectory_marker_.colors.push_back(c);
                trajectory_marker_.header.stamp = msg->header.stamp;
                trajectory_publisher_.publish(trajectory_marker_);
            }
        }
        else
            std::cout << "cam tracker not ready!" << std::endl;

        /*std_msgs::Float32 conf_mesage;
      conf_mesage.data = conf;
      confidence_publisher_.publish(conf_mesage);*/
    }

/**
 * @brief Callback for feedback subscriber for getting the transformation of moved markers
 *
 * @param feedback subscribed from geometry_map/map/feedback
 */
void ShapeVisualization::setShapePosition(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback)//,const cob_3d_mapping_msgs::Shape& shape)
{

  cob_3d_mapping_msgs::ShapeArray map_msg;
  map_msg.header.frame_id="/map";
  map_msg.header.stamp = ros::Time::now();

  int shape_id,index;
  index=-1;
  stringstream name(feedback->marker_name);

  Eigen::Quaternionf quat;

  Eigen::Matrix3f rotationMat;
  Eigen::MatrixXf rotationMatInit;

  Eigen::Vector3f normalVec;
  Eigen::Vector3f normalVecNew;
  Eigen::Vector3f newCentroid;
  Eigen::Matrix4f transSecondStep;


  if (feedback->marker_name != "Text"){
    name >> shape_id ;
    cob_3d_mapping::Polygon p;

    for(int i=0;i<sha.shapes.size();++i)
    {
    	if (sha.shapes[i].id == shape_id)
	{
		index = i;
	}

    }


    // temporary fix.
    //do nothing if index of shape is not found
    // this is not supposed to occur , but apparently it does
    if(index==-1){

    ROS_WARN("shape not in map array");
    return;
	}


    cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

    if (feedback->event_type == 2 && feedback->menu_entry_id == 5){
      quatInit.x() = (float)feedback->pose.orientation.x ;           //normalized
      quatInit.y() = (float)feedback->pose.orientation.y ;
      quatInit.z() = (float)feedback->pose.orientation.z ;
      quatInit.w() = (float)feedback->pose.orientation.w ;

      oldCentroid (0) = (float)feedback->pose.position.x ;
      oldCentroid (1) = (float)feedback->pose.position.y ;
      oldCentroid (2) = (float)feedback->pose.position.z ;

      quatInit.normalize() ;

      rotationMatInit = quatInit.toRotationMatrix() ;

      transInit.block(0,0,3,3) << rotationMatInit ;
      transInit.col(3).head(3) << oldCentroid(0) , oldCentroid(1), oldCentroid(2) ;
      transInit.row(3) << 0,0,0,1 ;

      transInitInv = transInit.inverse() ;
      Eigen::Affine3f affineInitFinal (transInitInv) ;
      affineInit = affineInitFinal ;

      std::cout << "transInit : " << "\n"    << affineInitFinal.matrix() << "\n" ;
    }

    if (feedback->event_type == 5){
      /* the name of the marker is arrows_shape_.id, we need to erase the "arrows_" part */
      //string strName(feedback->marker_name);
      //strName.erase(strName.begin(),strName.begin()+7);
//      stringstream name(strName);
	stringstream name(feedback->marker_name);

      name >> shape_id ;
      cob_3d_mapping::Polygon p;
      cob_3d_mapping::fromROSMsg (sha.shapes.at(index), p);

      quat.x() = (float)feedback->pose.orientation.x ;           //normalized
      quat.y() = (float)feedback->pose.orientation.y ;
      quat.z() = (float)feedback->pose.orientation.z ;
      quat.w() = (float)feedback->pose.orientation.w ;

      quat.normalize() ;

      rotationMat = quat.toRotationMatrix() ;

      normalVec << sha.shapes.at(index).params[0],                   //normalized
          sha.shapes.at(index).params[1],
//.........这里部分代码省略.........

Eigen::Matrix4f Particle::translationnMatrix() const
{
	Eigen::Matrix4f mat = Eigen::Matrix4f::Identity();
	mat.col(3) = Eigen::Vector4f(m_position.x(), m_position.y(), m_position.z(), 1.0);
	return mat;
}