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