ROS中的点云处理

使用点云库(Point Cloud Library, pcl)可以三维图像传感器的数据。这个开源库是独立的，但是可以在ROS中使用。pcl中提供了一系列函数来处理三维数据。本文使用一些常用的函数介绍一些简单的功能。

简单的点云显示节点

pcl_utils包中包含了源文件display_ellipse.cpp和相关的模块make_clouds.cpp。该程序介绍了PCL基础数据类型及其与ROS兼容消息的转换。主要是在xy平面建立了一个椭圆，然后沿着z轴拉伸，颜色随z轴红绿蓝渐变。具体代码如下：

//make_clouds.cpp
//a function to populate two point clouds with computed points
// modified from: from: http://docs.ros.org/hydro/api/pcl/html/pcl__visualizer__demo_8cpp_source.html
#include 
#include 
#include 

#include  //ROS message type to publish a pointCloud
#include  //use these to convert between PCL and ROS datatypes
#include 

#include 
#include 


using namespace std;

//a function to populate a pointCloud and a colored pointCloud;
// provide pointers to these, and this function will fill them with data
void make_clouds(pcl::PointCloud::Ptr basic_cloud_ptr,
        pcl::PointCloud::Ptr point_cloud_ptr) {
    // make an ellipse extruded along the z-axis. The color for
    // the XYZRGB cloud will gradually go from red to green to blue.

    uint8_t r(255), g(15), b(15); //declare and initialize red, green, blue component values

    //here are "point" objects that are compatible as building-blocks of point clouds
    pcl::PointXYZ basic_point; // simple points have x,y,z, but no color
    pcl::PointXYZRGB point; //colored point clouds also have RGB values

    for (float z = -1.0; z <= 1.0; z += 0.05) //build cloud in z direction
    {
        // color is encoded strangely, but efficiently.  Stored as a 4-byte "float", but
        // interpreted as individual byte values for 3 colors
        // bits 0-7 are blue value, bits 8-15 are green, bits 16-23 are red; 
        // Can build the rgb encoding with bit-level operations:
        uint32_t rgb = (static_cast (r) << 16 |
                static_cast (g) << 8 | static_cast (b));

        // and encode these bits as a single-precision (4-byte) float:
        float rgb_float = *reinterpret_cast<float*> (&rgb);

        //using fixed color and fixed z, compute coords of an ellipse in x-y plane
        for (float ang = 0.0; ang <= 2.0 * M_PI; ang += 2.0 * M_PI / 72.0) {
            //choose minor axis length= 0.5, major axis length = 1.0
            // compute and fill in components of point
            basic_point.x = 0.5 * cosf(ang); //cosf is cosine, operates on and returns single-precision floats
            basic_point.y = sinf(ang);
            basic_point.z = z;
            basic_cloud_ptr->points.push_back(basic_point); //append this point to the vector of points

            //use the same point coordinates for our colored pointcloud      
            point.x = basic_point.x;
            point.y = basic_point.y;
            point.z = basic_point.z;
            //but also add rgb information
            point.rgb = rgb_float; //*reinterpret_cast (&rgb);
            point_cloud_ptr->points.push_back(point);
        }
        if (z < 0.0) //alter the color smoothly in the z direction
        {
            r -= 12; //less red
            g += 12; //more green
        } else {
            g -= 12; // for positive z, lower the green
            b += 12; // and increase the blue
        }
    }

    //these will be unordered point clouds, i.e. a random bucket of points
    basic_cloud_ptr->width = (int) basic_cloud_ptr->points.size();
    basic_cloud_ptr->height = 1; //height=1 implies this is not an "ordered" point cloud
    basic_cloud_ptr->header.frame_id = "camera"; // need to assign a frame id

    point_cloud_ptr->width = (int) point_cloud_ptr->points.size();
    point_cloud_ptr->height = 1;
    point_cloud_ptr->header.frame_id = "camera";        

} 

makeclouds()函数接收PCL对象指针，然后用数据给他赋值。pcl::PointCloud适合不同类型的数据，pcl::PointCloud支持带有颜色的点云。第一个参数是指向简单点云的指针，不包含亮度颜色等信息，第二个参数是指向带有颜色信息的点云。

display_ellipse.cpp演示了如何将点云图像保存在硬盘中，还有向话题发布点云信息，通过rviz可视化。具体代码如下：

//display_ellipse.cpp
//example of creating a point cloud and publishing it for rviz display
//wsn March 2016

#include<ros/ros.h> //generic C++ stuff
#include <stdlib.h>
#include <math.h>

#include <sensor_msgs/PointCloud2.h> //ROS message type to publish a pointCloud
#include <pcl_ros/point_cloud.h> //use these to convert between PCL and ROS datatypes
#include <pcl/ros/conversions.h>

#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl-1.7/pcl/PCLHeader.h>


using namespace std;

//this function is defined in: make_clouds.cpp
extern void make_clouds(pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr,
        pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_ptr);

int main(int argc, char** argv) {
    ros::init(argc, argv, "ellipse"); //node name
    ros::NodeHandle nh;

    // create some point-cloud objects to hold data
    pcl::PointCloud<pcl::PointXYZ>::Ptr basic_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); //no color
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr point_cloud_clr_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); //colored

    cout << "Generating example point-cloud ellipse.\n\n";
    cout << "view in rviz; choose: topic= ellipse; and fixed frame= camera" << endl;

    // -----use fnc to create example point clouds: basic and colored-----
    make_clouds(basic_cloud_ptr, point_cloud_clr_ptr);
    pcl::io::savePCDFileASCII ("ellipse.pcd", *point_cloud_clr_ptr); //save image to disk
    // we now have "interesting" point clouds in basic_cloud_ptr and point_cloud_clr_ptr
    // 将带有颜色的点云保存

    sensor_msgs::PointCloud2 ros_cloud; //here is the ROS-compatible pointCloud message
    //we'll publish the colored point cloud; 
    pcl::toROSMsg(*point_cloud_clr_ptr, ros_cloud); //convert from PCL to ROS type this way

    //let's publish the colored point cloud in a ROS-compatible message; 
    // we'll publish to topic "ellipse"
    ros::Publisher pubCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ellipse", 1);

    //publish the ROS-type message; can view this in rviz on topic "/ellipse"
    //BUT need to set the Rviz fixed frame to "camera"
    while (ros::ok()) {
        pubCloud.publish(ros_cloud);
        ros::Duration(0.5).sleep(); //keep refreshing the publication periodically
    }
    return 0;
}

运行命令

rosrun pcl_utils display_ellipse

在rviz中进行相关配置，可以观察到点云

硬盘中加载并显示点云

display_pcd_file.cpp文件演示了在硬盘中读取pcd数据并在rviz中显示的例子。内容如下：

//display_pcd_file.cpp
// prompts for a pcd file name, reads the file, and displays to rviz on topic "pcd"
//wsn March 2016

#include //generic C++ stuff
#include 
#include 
#include  //useful ROS message types
#include  //to convert between PCL a nd ROS
#include 
#include 
#include 
#include 
#include 
#include 

using namespace std;

int main(int argc, char** argv) {
    ros::init(argc, argv, "pcd_publisher"); //node name
    ros::NodeHandle nh; 
    pcl::PointCloud::Ptr pcl_clr_ptr(new pcl::PointCloud); //pointer for color version of pointcloud

    cout<<"enter pcd file name: ";
    string fname;
    cin>>fname;

    if (pcl::io::loadPCDFile (fname, *pcl_clr_ptr) == -1) //* load the file
  {
    ROS_ERROR ("Couldn't read file \n");
    return (-1);
  }
  std::cout << "Loaded "
            << pcl_clr_ptr->width * pcl_clr_ptr->height
            << " data points from file "<std::endl;

   //publish the point cloud in a ROS-compatible message; here's a publisher:
    ros::Publisher pubCloud = nh.advertise ("/pcd", 1);
    sensor_msgs::PointCloud2 ros_cloud;  //here is the ROS-compatible message
    pcl::toROSMsg(*pcl_clr_ptr, ros_cloud); //convert from PCL to ROS type this way
    ros_cloud.header.frame_id = "camera_depth_optical_frame";
    cout << "view in rviz; choose: topic= pcd; and fixed frame= camera_depth_optical_frame" << endl;
    //publish the ROS-type message on topic "/ellipse"; can view this in rviz
    while (ros::ok()) {

        pubCloud.publish(ros_cloud);
        ros::spinOnce();
        ros::Duration(0.1).sleep();
    }
    return 0;
}

运行

rosrun pcl_utils display_pcd _file

在命令行输入coke_can.pcd(在pcd_image文件夹中找)，可以得到

使用PCL方法解读点云图像

find_plane_pcd_file.cpp文件中演示了PCL方法的使用。还是针对pcd文件进行的。

//find_plane_pcd_file.cpp
// prompts for a pcd file name, reads the file, and displays to rviz on topic "pcd"
// can select a patch; then computes a plane containing that patch, which is published on topic "planar_pts"
// illustrates use of PCL methods: computePointNormal(), transformPointCloud(), 
// pcl::PassThrough methods setInputCloud(), setFilterFieldName(), setFilterLimits, filter()
// pcl::io::loadPCDFile() 
// pcl::toROSMsg() for converting PCL pointcloud to ROS message
// voxel-grid filtering: pcl::VoxelGrid,  setInputCloud(), setLeafSize(), filter()
//wsn March 2016

#include<ros/ros.h> 
#include <stdlib.h>
#include <math.h>

#include <sensor_msgs/PointCloud2.h> 
#include <pcl_ros/point_cloud.h> //to convert between PCL and ROS
#include <pcl/ros/conversions.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
//#include  //PCL is migrating to PointCloud2 

#include <pcl/common/common_headers.h>
#include <pcl-1.7/pcl/point_cloud.h>
#include <pcl-1.7/pcl/PCLHeader.h>

//will use filter objects "passthrough" and "voxel_grid" in this example
#include <pcl/filters/passthrough.h>
#include <pcl/filters/voxel_grid.h> 

#include <pcl_utils/pcl_utils.h>  //a local library with some utility fncs


using namespace std;
extern PclUtils *g_pcl_utils_ptr;

//this fnc is defined in a separate module, find_indices_of_plane_from_patch.cpp
extern void find_indices_of_plane_from_patch(pcl::PointCloud<pcl::PointXYZRGB>::Ptr input_cloud_ptr,
        pcl::PointCloud<pcl::PointXYZ>::Ptr patch_cloud_ptr, vector<int> &indices);

int main(int argc, char** argv) {
    ros::init(argc, argv, "plane_finder"); //node name
    ros::NodeHandle nh;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclKinect_clr_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); //pointer for color version of pointcloud
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr plane_pts_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); //pointer for pointcloud of planar points found
    pcl::PointCloud<pcl::PointXYZ>::Ptr selected_pts_cloud_ptr(new pcl::PointCloud<pcl::PointXYZ>); //ptr to selected pts from Rvis tool
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr downsampled_kinect_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); //ptr to hold filtered Kinect image
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr box_filtered_pts_ptr(new pcl::PointCloud<pcl::PointXYZRGB>); //ptr to hold filtered Kinect image

    vector<int> indices;

    //load a PCD file using pcl::io function; alternatively, could subscribe to Kinect messages    
    string fname;
    cout << "enter pcd file name: "; //prompt to enter file name
    cin >> fname;
    if (pcl::io::loadPCDFile<pcl::PointXYZRGB> (fname, *pclKinect_clr_ptr) == -1) //* load the file
    {
        ROS_ERROR("Couldn't read file \n");
        return (-1);
    }
    //PCD file does not seem to record the reference frame;  set frame_id manually
    pclKinect_clr_ptr->header.frame_id = "camera_depth_optical_frame";
    ROS_INFO("view frame camera_depth_optical_frame on topics pcd, planar_pts and downsampled_pcd");

    //will publish  pointClouds as ROS-compatible messages; create publishers; note topics for rviz viewing
    ros::Publisher pubCloud = nh.advertise<sensor_msgs::PointCloud2> ("/pcd", 1);
    ros::Publisher pubPlane = nh.advertise<sensor_msgs::PointCloud2> ("planar_pts", 1);
    ros::Publisher pubDnSamp = nh.advertise<sensor_msgs::PointCloud2> ("downsampled_pcd", 1);

    sensor_msgs::PointCloud2 ros_cloud, ros_planar_cloud, downsampled_cloud; //here are ROS-compatible messages
    pcl::toROSMsg(*pclKinect_clr_ptr, ros_cloud); //convert from PCL cloud to ROS message this way

    //use voxel filtering to downsample the original cloud:
    cout << "starting voxel filtering" << endl;
    pcl::VoxelGrid<pcl::PointXYZRGB> vox;
    vox.setInputCloud(pclKinect_clr_ptr);

    vox.setLeafSize(0.02f, 0.02f, 0.02f);
    vox.filter(*downsampled_kinect_ptr);
    cout << "done voxel filtering" << endl;

    cout << "num bytes in original cloud data = " << pclKinect_clr_ptr->points.size() << endl;
    cout << "num bytes in filtered cloud data = " << downsampled_kinect_ptr->points.size() << endl; // ->data.size()<
    pcl::toROSMsg(*downsampled_kinect_ptr, downsampled_cloud); //convert to ros message for publication and display

    PclUtils pclUtils(&nh); //instantiate a PclUtils object--a local library w/ some handy fncs
    g_pcl_utils_ptr = &pclUtils; // make this object shared globally, so above fnc can use it too

    Eigen::Vector3f crop_pt_min, crop_pt_max;
    crop_pt_min << 0, -0.5, 0.02;
    crop_pt_max << 2.0, 0.5, 0.5;
    cout << " select a patch of points to find corresponding plane..." << endl; //prompt user action
    //loop to test for new selected-points inputs and compute and display corresponding planar fits 
    while (ros::ok()) {
        if (pclUtils.got_selected_points()) { //here if user selected a new patch of points
            pclUtils.reset_got_selected_points(); // reset for a future trigger
            pclUtils.get_copy_selected_points(selected_pts_cloud_ptr); //get a copy of the selected points
            cout << "got new patch with number of selected pts = " << selected_pts_cloud_ptr->points.size() << endl;
            //find pts coplanar w/ selected patch, using PCL methods in above-defined function
            //"indices" will get filled with indices of points that are approx co-planar with the selected patch
            // can extract indices from original cloud, or from voxel-filtered (down-sampled) cloud
            find_indices_of_plane_from_patch(pclKinect_clr_ptr, selected_pts_cloud_ptr, indices);
            pcl::copyPointCloud(*pclKinect_clr_ptr, indices, *plane_pts_ptr);
            //the new cloud is a set of points from original cloud, coplanar with selected patch; display the result
            pcl::toROSMsg(*plane_pts_ptr, ros_planar_cloud); //convert to ros message for publication and display
        }
        pubCloud.publish(ros_cloud); // will not need to keep republishing if display setting is persistent
        pubPlane.publish(ros_planar_cloud); // display the set of points computed to be coplanar w/ selection
        pubDnSamp.publish(downsampled_cloud); //can directly publish a pcl::PointCloud2!!
        ros::spinOnce(); //pclUtils needs some spin cycles to invoke callbacks for new selected points
        ros::Duration(0.3).sleep();
    }

    return 0;
}

可以查看降采样效果

选择交互中，选择的那个插件有些问题，没能实现

寻找物体

采用视觉的方式，利用点云可以实现物体识别。使用以下命令在咖啡桌上放一个长条块，搭建gazebo环境

roslaunch worlds table_w_block.launch

然后用下列命令加入Kinect模块，获取环境中的点云信息

roslaunch simple_camera_model kinect_simu2.launch

上述两个命令可以观察到如下两个界面，一个gazebo一个rviz。

使用以下命令开启查找物体服务器

rosrun object_finder object_finder_as

使用以下命令添加一个Marker用于显示找到的物体的位姿

rosrun example_rviz_marker triad_display

使用以下命令进行物体的查找

rosrun object_finder example_object_finder_action_client

得到的结果如下

[ INFO] [1519698957.362366004, 665.979000000]: got pose x,y,z = 0.497095, -0.347294, 0.791365
[ INFO] [1519698957.362389082, 665.979000000]: got quaternion x,y,z, w = -0.027704, 0.017787, -0.540053, 0.840936

是先找到桌面又找到物体的。
通过下图理解一下整个程序的流程

/triad_display节点接收/triad_display_pose消息，用于显示三维标记的位姿；
/rcamera_frame_bdcst节点发布/tf消息，表示camera_link与kinect_depth_frame之间的转换关系；
/kinect_broadcaster2节点发布/tf消息，表示kinect_link与kinect_pc_frame之间的坐标转换关系；
/robot_state_publisher发布/tf_static消息，表示长条小块的位姿信息；
/gazebo发布点云数据/kinect/depth/points；
/object_finder_node节点作为查找物体的服务器，接收各方面信息，发布查找结果；
/example_object_finder_action_client节点作为查找物体的客户端，发送查找请求，接收查找结果，发布位姿信息。

查找的过程主要是找平面，找平面的方法是：

//version that includes x, y and z limits
double PclUtils::find_table_height(double x_min, double x_max, double y_min, double y_max, double z_min, double z_max, double dz_tol) {//在一个规定的立方体范围内，以dz_tol为步长，查找桌面
    vector<int> indices;
    pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
    pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud);
    pcl::PassThrough pass; //create a pass-through object    
    pass.setInputCloud(pclTransformed_ptr_); //set the cloud we want to operate on--pass via a pointer
    pass.setFilterFieldName("x"); // we will "filter" based on points that lie within some range of z-value
    pass.setFilterLimits(x_min, x_max);
    pass.filter(*cloud_filtered); //fill this cloud with result
    int n_filtered = cloud_filtered->points.size();
    ROS_INFO("num x-filtered pts = %d",n_filtered);
    //pass.filter(*inliers);
    //pass.setIndices(inliers);
    pass.setInputCloud(cloud_filtered);
    pass.setFilterFieldName("y"); // we will "filter" based on points that lie within some range of z-value
    pass.setFilterLimits(y_min, y_max);
    pass.filter(*cloud_filtered);
    n_filtered = cloud_filtered->points.size();
    ROS_INFO("num y-filtered pts = %d",n_filtered);
    //pass.setIndices(indices);

    pass.setInputCloud(cloud_filtered);
    pass.setFilterFieldName("z"); // we will "filter" based on points that lie within some range of z-value
    pass.setFilterLimits(z_min, z_max);
    pass.filter(*cloud_filtered);
    n_filtered = cloud_filtered->points.size();
    ROS_INFO("num z-filtered pts = %d",n_filtered);

    pass.setInputCloud(cloud_filtered);    
    double z_table = 0.0;
    int npts_max = 0;
    int npts;
    for (double z = z_min; z < z_max; z += dz_tol) {
        pass.setFilterLimits(z, z + dz_tol);
        pass.filter(indices); //  this will return the indices of the points in   transformed_cloud_ptr that pass our test
        npts = indices.size();
        ROS_INFO("z=%f; npts = %d",z,npts);
        if (npts > npts_max) {
            npts_max = npts;//找到垂直于z轴点数最多的面
            z_table = z + 0.5 * dz_tol;
        }
        ROS_DEBUG("number of points passing the filter = %d", npts);
    }
   return z_table;
}

//given table height and known object height, filter transformed points to find points within x, y and z bounds,
//给定了桌面高度，有已知物体高度，在限定立方体中找到物体上表面
// presumably extracting points on the top surface of the object of interest
// fit a plane to the surviving points and find normal and major axis
const int min_n_filtered = 100;
bool PclUtils::find_plane_fit(double x_min, double x_max, double y_min, double y_max, double z_min, double z_max, double dz_tol,
     Eigen::Vector3f &plane_normal, double &plane_dist, Eigen::Vector3f &major_axis, Eigen::Vector3f  ¢roid) { 
    vector<int> indices;
    bool ans_valid = true;
    pcl::PointCloud::Ptr cloud_filtered (new pcl::PointCloud);
    pcl::PassThrough pass; //create a pass-through object    
    pass.setInputCloud(pclTransformed_ptr_); //set the cloud we want to operate on--pass via a pointer
    pass.setFilterFieldName("x"); // we will "filter" based on points that lie within some range of z-value
    pass.setFilterLimits(x_min, x_max);
    pass.filter(*cloud_filtered); //fill this cloud with result
    int n_filtered = cloud_filtered->points.size();
    ROS_INFO("num x-filtered pts = %d",n_filtered);
    //pass.filter(*inliers);
    //pass.setIndices(inliers);
    pass.setInputCloud(cloud_filtered);
    pass.setFilterFieldName("y"); // we will "filter" based on points that lie within some range of z-value
    pass.setFilterLimits(y_min, y_max);
    pass.filter(*cloud_filtered);
    n_filtered = cloud_filtered->points.size();
    ROS_INFO("num y-filtered pts = %d",n_filtered);
    //pass.setIndices(indices);

    pass.setInputCloud(cloud_filtered);
    pass.setFilterFieldName("z"); // we will "filter" based on points that lie within some range of z-value
    pass.setFilterLimits(z_min, z_max);
    pass.filter(*cloud_filtered);
    n_filtered = cloud_filtered->points.size();
    ROS_INFO("num z-filtered pts = %d",n_filtered);
    if (n_filteredfalse; //give warning of insufficient data
    }
    fit_points_to_plane(cloud_filtered, plane_normal, plane_dist);    
    major_axis = major_axis_;
    centroid = centroid_;
    return ans_valid;
}

void PclUtils::fit_points_to_plane(Eigen::MatrixXf points_mat, Eigen::Vector3f &plane_normal, double &plane_dist) {
    //ROS_INFO("starting identification of plane from data: ");
    int npts = points_mat.cols(); // number of points = number of columns in matrix; check the size

    // first compute the centroid of the data:
    //Eigen::Vector3f centroid; // make this member var, centroid_
    centroid_ = Eigen::MatrixXf::Zero(3, 1); // see http://eigen.tuxfamily.org/dox/AsciiQuickReference.txt

    //centroid = compute_centroid(points_mat);
     for (int ipt = 0; ipt < npts; ipt++) {
        centroid_ += points_mat.col(ipt); //add all the column vectors together
    }
    centroid_ /= npts; //divide by the number of points to get the centroid    
    cout<<"centroid: "<// subtract this centroid from all points in points_mat:
    Eigen::MatrixXf points_offset_mat = points_mat;
    for (int ipt = 0; ipt < npts; ipt++) {
        points_offset_mat.col(ipt) = points_offset_mat.col(ipt) - centroid_;
    }//相当于将目标块中心平移到原点
    //计算协方差3×3矩阵
    //compute the covariance matrix w/rt x,y,z:
    Eigen::Matrix3f CoVar;
    CoVar = points_offset_mat * (points_offset_mat.transpose()); //3xN matrix times Nx3 matrix is 3x3
    //cout<<"covariance: "<
    //cout<

    // here is a more complex object: a solver for eigenvalues/eigenvectors;
    // we will initialize it with our covariance matrix, which will induce computing eval/evec pairs
    Eigen::EigenSolver es3f(CoVar);

    Eigen::VectorXf evals; //we'll extract the eigenvalues to here
    //cout<<"size of evals: "<
    //cout<<"rows,cols = "<
    //cout << "The eigenvalues of CoVar are:" << endl << es3d.eigenvalues().transpose() << endl;
    //cout<<"(these should be real numbers, and one of them should be zero)"<
    //cout << "The matrix of eigenvectors, V, is:" << endl;
    //cout<< es3d.eigenvectors() << endl << endl;
    //cout<< "(these should be real-valued vectors)"<
    // in general, the eigenvalues/eigenvectors can be complex numbers
    //however, since our matrix is self-adjoint (symmetric, positive semi-definite), we expect
    // real-valued evals/evecs;  we'll need to strip off the real parts of the solution

    evals = es3f.eigenvalues().real(); //协方差矩阵的特征值 grab just the real parts
    //cout<<"real parts of evals: "<

    // our solution should correspond to an e-val of zero, which will be the minimum eval
    //  (all other evals for the covariance matrix will be >0)
    // however, the solution does not order the evals, so we'll have to find the one of interest ourselves

    double min_lambda = evals[0]; //initialize the hunt for min eval
    double max_lambda = evals[0]; // and for max eval
    //Eigen::Vector3cf complex_vec; // here is a 3x1 vector of double-precision, complex numbers
    //Eigen::Vector3f evec0, evec1, evec2; //, major_axis; 
    //evec0 = es3f.eigenvectors().col(0).real();
    //evec1 = es3f.eigenvectors().col(1).real();
    //evec2 = es3f.eigenvectors().col(2).real();  


    //((pt-centroid)*evec)*2 = evec'*points_offset_mat'*points_offset_mat*evec = 
    // = evec'*CoVar*evec = evec'*lambda*evec = lambda
    // min lambda is ideally zero for evec= plane_normal, since points_offset_mat*plane_normal~= 0
    // max lambda is associated with direction of major axis

    //sort the evals:

    //complex_vec = es3f.eigenvectors().col(0); // here's the first e-vec, corresponding to first e-val
    //cout<<"complex_vec: "<
    //cout<
    plane_normal = es3f.eigenvectors().col(0).real(); //complex_vec.real(); //strip off the real part
    major_axis_ = es3f.eigenvectors().col(0).real(); // starting assumptions

    //cout<<"real part: "<
    //est_plane_normal = es3d.eigenvectors().col(0).real(); // evecs in columns

    double lambda_test;
    int i_normal = 0;
    int i_major_axis=0;
    //loop through "all" ("both", in this 3-D case) the rest of the solns, seeking min e-val
    for (int ivec = 1; ivec < 3; ivec++) {
        lambda_test = evals[ivec];
        if (lambda_test < min_lambda) {//最小特征值对应着垂直于平面的方向
            min_lambda = lambda_test;
            i_normal = ivec; //this index is closer to index of min eval
            plane_normal = es3f.eigenvectors().col(i_normal).real();
        }
        if (lambda_test > max_lambda) {//最大特征值对应于主轴方向
            max_lambda = lambda_test;
            i_major_axis = ivec; //this index is closer to index of min eval
            major_axis_ = es3f.eigenvectors().col(i_major_axis).real();
        }        
    }
    // at this point, we have the minimum eval in "min_lambda", and the plane normal
    // (corresponding evec) in "est_plane_normal"/
    // these correspond to the ith entry of i_normal
    //cout<<"min eval is "<
    //cout<<"corresponding evec (est plane normal): "<
    //cout<<"max eval is "<
    //cout<<"corresponding evec (est major axis): "<

    //what is the correct sign of the normal?  If the data is with respect to the camera frame,
    // then the camera optical axis is z axis, and thus any points reflected must be from a surface
    // with negative z component of surface normal
    if (plane_normal(2)>0) plane_normal = -plane_normal; // negate, if necessary

    //cout<<"correct answer is: "<
    plane_dist = plane_normal.dot(centroid_);
    //cout<<"est plane distance from origin = "<
    //cout<<"correct answer is: "<
    //cout<
    ROS_INFO("major_axis: %f, %f, %f",major_axis_(0),major_axis_(1),major_axis_(2));
    ROS_INFO("plane normal: %f, %f, %f",plane_normal(0),plane_normal(1),plane_normal(2));
}

//get pts from cloud, pack the points into an Eigen::MatrixXf, then use above
// fit_points_to_plane fnc

void PclUtils::fit_points_to_plane(pcl::PointCloud::Ptr input_cloud_ptr, Eigen::Vector3f &plane_normal, double &plane_dist) {
    Eigen::MatrixXf points_mat;
    Eigen::Vector3f cloud_pt;
    //populate points_mat from cloud data;

    int npts = input_cloud_ptr->points.size();
    points_mat.resize(3, npts);

    //somewhat odd notation: getVector3fMap() reading OR WRITING points from/to a pointcloud, with conversions to/from Eigen
    for (int i = 0; i < npts; ++i) {
        cloud_pt = input_cloud_ptr->points[i].getVector3fMap();
        points_mat.col(i) = cloud_pt;
    }
    fit_points_to_plane(points_mat, plane_normal, plane_dist);

}

---