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
00031
00032
00033
00034
00035
00036
00037
00038 #include "pcl/impl/instantiate.hpp"
00039 #include "pcl/point_types.h"
00040 #include "pcl/filters/project_inliers.h"
00041 #include "pcl/filters/impl/project_inliers.hpp"
00042
00044 void
00045 pcl::ProjectInliers<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
00046 {
00047 if (indices_->empty ())
00048 {
00049 ROS_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!", getClassName ().c_str ());
00050 output.width = output.height = 0;
00051 output.data.clear ();
00052 return;
00053 }
00054
00055
00056
00057 Eigen::VectorXf model_coefficients (model_->values.size ());
00058 for (size_t i = 0; i < model_->values.size (); ++i)
00059 model_coefficients[i] = model_->values[i];
00060
00061
00062 if (!initSACModel (model_type_))
00063 {
00064 ROS_ERROR ("[pcl::%s::segment] Error initializing the SAC model!", getClassName ().c_str ());
00065 output.width = output.height = 0;
00066 output.data.clear ();
00067 return;
00068 }
00069 pcl::PointCloud<pcl::PointXYZ> cloud_out;
00070
00071 if (!copy_all_fields_)
00072 sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, false);
00073 else
00074 sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, true);
00075
00076 if (copy_all_data_)
00077 {
00078 output.height = input_->height;
00079 output.width = input_->width;
00080 output.is_bigendian = input_->is_bigendian;
00081 output.point_step = input_->point_step;
00082 output.row_step = input_->row_step;
00083 output.data = input_->data;
00084 output.is_dense = input_->is_dense;
00085
00086
00087 int x_idx = pcl::getFieldIndex (output, "x");
00088 int y_idx = pcl::getFieldIndex (output, "y");
00089 int z_idx = pcl::getFieldIndex (output, "z");
00090
00091
00092 for (size_t i = 0; i < indices_->size (); ++i)
00093 {
00094 memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
00095 memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
00096 memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
00097 }
00098 }
00099 else
00100 {
00101 if (!copy_all_fields_)
00102 {
00103 pcl::toROSMsg<pcl::PointXYZ> (cloud_out, output);
00104 }
00105 else
00106 {
00107
00108 output.height = 1;
00109 output.width = indices_->size ();
00110 output.point_step = input_->point_step;
00111 output.data.resize (output.width * output.point_step);
00112 output.is_bigendian = input_->is_bigendian;
00113 output.row_step = output.point_step * output.width;
00114
00115 output.is_dense = true;
00116
00117
00118 int x_idx = pcl::getFieldIndex (output, "x");
00119 int y_idx = pcl::getFieldIndex (output, "y");
00120 int z_idx = pcl::getFieldIndex (output, "z");
00121
00122 for (size_t i = 0; i < indices_->size (); ++i)
00123 {
00124 memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * input_->point_step], output.point_step);
00125 memcpy (&output.data[i * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
00126 memcpy (&output.data[i * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
00127 memcpy (&output.data[i * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
00128 }
00129 }
00130 }
00131 }
00132
00134 bool
00135 pcl::ProjectInliers<sensor_msgs::PointCloud2>::initSACModel (int model_type)
00136 {
00137
00138 PointCloud<PointXYZ> cloud;
00139 fromROSMsg (*input_, cloud);
00140 PointCloud<PointXYZ>::Ptr cloud_ptr = cloud.makeShared ();
00141
00142
00143 switch (model_type)
00144 {
00145 case SACMODEL_PLANE:
00146 {
00147
00148 sacmodel_.reset (new SampleConsensusModelPlane<pcl::PointXYZ> (cloud_ptr));
00149 break;
00150 }
00151 case SACMODEL_LINE:
00152 {
00153
00154 sacmodel_.reset (new SampleConsensusModelLine<pcl::PointXYZ> (cloud_ptr));
00155 break;
00156 }
00157 case SACMODEL_CIRCLE2D:
00158 {
00159
00160 sacmodel_.reset (new SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_ptr));
00161 break;
00162 }
00163 case SACMODEL_SPHERE:
00164 {
00165
00166 sacmodel_.reset (new SampleConsensusModelSphere<pcl::PointXYZ> (cloud_ptr));
00167 break;
00168 }
00169 case SACMODEL_PARALLEL_LINE:
00170 {
00171
00172 sacmodel_.reset (new SampleConsensusModelParallelLine<pcl::PointXYZ> (cloud_ptr));
00173 break;
00174 }
00175 case SACMODEL_PERPENDICULAR_PLANE:
00176 {
00177
00178 sacmodel_.reset (new SampleConsensusModelPerpendicularPlane<pcl::PointXYZ> (cloud_ptr));
00179 break;
00180 }
00181 case SACMODEL_CYLINDER:
00182 {
00183
00184 sacmodel_.reset (new SampleConsensusModelCylinder<pcl::PointXYZ, Normal> (cloud_ptr));
00185 break;
00186 }
00187 case SACMODEL_NORMAL_PLANE:
00188 {
00189
00190 sacmodel_.reset (new SampleConsensusModelNormalPlane<pcl::PointXYZ, Normal> (cloud_ptr));
00191 break;
00192 }
00193 case SACMODEL_NORMAL_PARALLEL_PLANE:
00194 {
00195
00196 sacmodel_.reset (new SampleConsensusModelNormalParallelPlane<pcl::PointXYZ, Normal> (cloud_ptr));
00197 break;
00198 }
00199 case SACMODEL_PARALLEL_PLANE:
00200 {
00201
00202 sacmodel_.reset (new SampleConsensusModelParallelPlane<pcl::PointXYZ> (cloud_ptr));
00203 break;
00204 }
00205 default:
00206 {
00207 ROS_ERROR ("[pcl::%s::initSACModel] No valid model given!", getClassName ().c_str ());
00208 return (false);
00209 }
00210 }
00211 return (true);
00212 }
00213
00214
00215 PCL_INSTANTIATE(ProjectInliers, PCL_XYZ_POINT_TYPES);
00216
