Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00039 #ifndef AR_TRACK_ALVAR_KINECT_FILTERING_H
00040 #define AR_TRACK_ALVAR_KINECT_FILTERING_H
00041
00042 #include <pcl_conversions/pcl_conversions.h>
00043 #include <pcl/point_types.h>
00044 #include <pcl/registration/icp.h>
00045 #include <pcl/registration/registration.h>
00046
00047 #include <geometry_msgs/PoseStamped.h>
00048 #include <ros/ros.h>
00049 #include <pcl/ModelCoefficients.h>
00050 #include <pcl/point_types.h>
00051 #include <pcl/sample_consensus/method_types.h>
00052 #include <pcl/sample_consensus/model_types.h>
00053 #include <pcl/segmentation/sac_segmentation.h>
00054 #include <pcl_ros/point_cloud.h>
00055 #include <pcl/filters/extract_indices.h>
00056 #include <boost/lexical_cast.hpp>
00057 #include <Eigen/StdVector>
00058
00059 #include <opencv2/core/core.hpp>
00060
00061 #include <tf/LinearMath/Matrix3x3.h>
00062
00063 namespace ar_track_alvar
00064 {
00065
00066 typedef pcl::PointXYZRGB ARPoint;
00067 typedef pcl::PointCloud<ARPoint> ARCloud;
00068
00069
00070 struct PlaneFitResult
00071 {
00072 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00073 PlaneFitResult () : inliers(ARCloud::Ptr(new ARCloud)) {}
00074 ARCloud::Ptr inliers;
00075 pcl::ModelCoefficients coeffs;
00076 };
00077
00078
00079 ARCloud::Ptr filterCloud (const ARCloud& cloud,
00080 const std::vector<cv::Point, Eigen::aligned_allocator<cv::Point> >& pixels);
00081
00082
00083 PlaneFitResult fitPlane (ARCloud::ConstPtr cloud);
00084
00085
00086
00087
00088
00089
00090 int
00091 extractOrientation (const pcl::ModelCoefficients& coeffs,
00092 const ARPoint& p1, const ARPoint& p2,
00093 const ARPoint& p3, const ARPoint& p4,
00094 geometry_msgs::Quaternion &retQ);
00095
00096
00097 int
00098 extractFrame (const pcl::ModelCoefficients& coeffs,
00099 const ARPoint& p1, const ARPoint& p2,
00100 const ARPoint& p3, const ARPoint& p4,
00101 tf::Matrix3x3 &retmat);
00102
00103
00104
00105 geometry_msgs::Point centroid (const ARCloud& points);
00106 }
00107
00108 #endif // include guard