36 template<
typename Po
intCloud2T>
39 if(cloud.data.empty())
44 int fieldStates[8] = {0};
45 #if PCL_VERSION_COMPARE(>=, 1, 10, 0) 50 for(
unsigned int i=0; i<cloud.fields.size(); ++i)
52 if(cloud.fields[i].name.compare(
"x") == 0)
55 fieldOffsets[0] = cloud.fields[i].offset;
57 else if(cloud.fields[i].name.compare(
"y") == 0)
60 fieldOffsets[1] = cloud.fields[i].offset;
62 else if(cloud.fields[i].name.compare(
"z") == 0 && !is2D)
65 fieldOffsets[2] = cloud.fields[i].offset;
67 else if(cloud.fields[i].name.compare(
"normal_x") == 0)
70 fieldOffsets[3] = cloud.fields[i].offset;
72 else if(cloud.fields[i].name.compare(
"normal_y") == 0)
75 fieldOffsets[4] = cloud.fields[i].offset;
77 else if(cloud.fields[i].name.compare(
"normal_z") == 0)
80 fieldOffsets[5] = cloud.fields[i].offset;
82 else if(cloud.fields[i].name.compare(
"rgb") == 0 || cloud.fields[i].name.compare(
"rgba") == 0)
85 fieldOffsets[6] = cloud.fields[i].offset;
87 else if(cloud.fields[i].name.compare(
"intensity") == 0)
89 if(cloud.fields[i].datatype != pcl::PCLPointField::FLOAT32)
91 static bool warningShown =
false;
94 UWARN(
"The input scan cloud has an \"intensity\" field " 95 "but the datatype (%d) is not supported. Intensity will be ignored. " 96 "This message is only shown once.", cloud.fields[i].datatype);
103 fieldOffsets[7] = cloud.fields[i].offset;
107 UDEBUG(
"Ignoring \"%s\" field", cloud.fields[i].name.c_str());
110 if(fieldStates[0]==0 || fieldStates[1]==0)
113 UERROR(
"Cloud has not corresponding fields to laser scan!");
117 bool hasNormals = fieldStates[3] || fieldStates[4] || fieldStates[5];
118 bool hasIntensity = fieldStates[7];
119 bool hasRGB = !hasIntensity&&fieldStates[6];
120 bool is3D = fieldStates[0] && fieldStates[1] && fieldStates[2];
123 int outputNormalOffset = 0;
126 if(hasNormals && hasIntensity)
129 outputNormalOffset = 4;
131 else if(hasNormals && !hasIntensity && !hasRGB)
134 outputNormalOffset = 3;
136 else if(hasNormals && hasRGB)
139 outputNormalOffset = 4;
141 else if(!hasNormals && hasIntensity)
145 else if(!hasNormals && hasRGB)
156 if(hasNormals && hasIntensity)
159 outputNormalOffset = 3;
161 else if(hasNormals && !hasIntensity)
164 outputNormalOffset = 2;
166 else if(!hasNormals && hasIntensity)
176 UASSERT(cloud.data.size()/cloud.point_step == (
uint32_t)cloud.height*cloud.width);
177 cv::Mat laserScan = cv::Mat(1, (
int)cloud.data.size()/cloud.point_step, CV_32FC(
LaserScan::channels(format)));
183 transformRot = transform.
rotation();
188 const uint8_t* row_data = &cloud.data[
row * cloud.row_step];
191 const uint8_t* msg_data = row_data + col * cloud.point_step;
193 float * ptr = laserScan.ptr<
float>(0, oi);
196 if(laserScan.channels() == 2)
198 ptr[0] = *(
float*)(msg_data + fieldOffsets[0]);
199 ptr[1] = *(
float*)(msg_data + fieldOffsets[1]);
202 else if(laserScan.channels() == 3)
204 ptr[0] = *(
float*)(msg_data + fieldOffsets[0]);
205 ptr[1] = *(
float*)(msg_data + fieldOffsets[1]);
208 ptr[2] = *(
float*)(msg_data + fieldOffsets[7]);
212 ptr[2] = *(
float*)(msg_data + fieldOffsets[2]);
216 else if(laserScan.channels() == 4)
218 ptr[0] = *(
float*)(msg_data + fieldOffsets[0]);
219 ptr[1] = *(
float*)(msg_data + fieldOffsets[1]);
220 ptr[2] = *(
float*)(msg_data + fieldOffsets[2]);
223 ptr[3] = *(
float*)(msg_data + fieldOffsets[7]);
227 #if PCL_VERSION_COMPARE(>=, 1, 10, 0) 236 int * ptrInt = (
int*)ptr;
237 ptrInt[3] = int(b) | (int(g) << 8) | (
int(r) << 16);
241 else if(laserScan.channels() == 5)
243 ptr[0] = *(
float*)(msg_data + fieldOffsets[0]);
244 ptr[1] = *(
float*)(msg_data + fieldOffsets[1]);
245 ptr[2] = *(
float*)(msg_data + fieldOffsets[3]);
246 ptr[3] = *(
float*)(msg_data + fieldOffsets[4]);
247 ptr[4] = *(
float*)(msg_data + fieldOffsets[5]);
250 else if(laserScan.channels() == 6)
252 ptr[0] = *(
float*)(msg_data + fieldOffsets[0]);
253 ptr[1] = *(
float*)(msg_data + fieldOffsets[1]);
256 ptr[2] = *(
float*)(msg_data + fieldOffsets[7]);
260 ptr[2] = *(
float*)(msg_data + fieldOffsets[2]);
262 ptr[3] = *(
float*)(msg_data + fieldOffsets[3]);
263 ptr[4] = *(
float*)(msg_data + fieldOffsets[4]);
264 ptr[5] = *(
float*)(msg_data + fieldOffsets[5]);
267 else if(laserScan.channels() == 7)
269 ptr[0] = *(
float*)(msg_data + fieldOffsets[0]);
270 ptr[1] = *(
float*)(msg_data + fieldOffsets[1]);
271 ptr[2] = *(
float*)(msg_data + fieldOffsets[2]);
274 ptr[3] = *(
float*)(msg_data + fieldOffsets[7]);
278 #if PCL_VERSION_COMPARE(>=, 1, 10, 0) 287 int * ptrInt = (
int*)ptr;
288 ptrInt[3] = int(b) | (int(g) << 8) | (
int(r) << 16);
290 ptr[4] = *(
float*)(msg_data + fieldOffsets[3]);
291 ptr[5] = *(
float*)(msg_data + fieldOffsets[4]);
292 ptr[6] = *(
float*)(msg_data + fieldOffsets[5]);
297 UFATAL(
"Cannot handle as many channels (%d)!", laserScan.channels());
300 if(!filterNaNs || valid)
302 if(valid && transformValid)
313 pt =
util3d::transformPoint(cv::Point3f(ptr[outputNormalOffset], ptr[outputNormalOffset+1], ptr[outputNormalOffset+2]), transformRot);
314 ptr[outputNormalOffset] = pt.x;
315 ptr[outputNormalOffset+1] = pt.y;
316 ptr[outputNormalOffset+2] = pt.z;
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
GLM_FUNC_DECL bool all(vecType< bool, P > const &v)
GLM_FUNC_DECL genType::row_type row(genType const &m, length_t const &index)
bool uIsFinite(const T &value)
#define UASSERT(condition)
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)