Drake Designer
 All Classes Namespaces Files Functions Variables Enumerations Enumerator Macros Pages
vtkMultisenseUtils.h
Go to the documentation of this file.
1 #ifndef __vtkMultisenseUtils_h
2 #define __vtkMultisenseUtils_h
3 
4 
5 #include <cstdio>
6 #include <iostream>
7 #include <string>
8 
9 #include <Eigen/Dense>
10 
11 #include <lcm/lcm-cpp.hpp>
12 
13 #include <bot_frames/bot_frames.h>
14 #include <bot_param/param_client.h>
15 
16 #include <lcmtypes/bot_core/planar_lidar_t.hpp>
17 #include <lcmtypes/multisense.hpp>
18 
19 
20 
21 #include <vtkSmartPointer.h>
22 #include <vtkXMLPolyDataWriter.h>
23 #include <vtkPolyData.h>
24 #include <vtkPointData.h>
25 #include <vtkPoints.h>
26 #include <vtkFloatArray.h>
27 #include <vtkUnsignedIntArray.h>
28 #include <vtkIdTypeArray.h>
29 #include <vtkCellArray.h>
30 #include <vtkTimerLog.h>
31 
32 
33 //----------------------------------------------------------------------------
34 namespace
35 {
36 
37 
38 class ScanLineData
39 {
40 public:
41 
42  uint64_t Revolution;
43  uint64_t ScanLineId;
44  double SpindleAngle;
45  Eigen::Isometry3d ScanToLocalStart;
46  Eigen::Isometry3d ScanToLocalEnd;
47  Eigen::Isometry3d BodyToLocalStart;
48  bot_core::planar_lidar_t msg;
49 };
50 
51 
52 class DataArrays
53 {
54 public:
55 
56  vtkSmartPointer<vtkPolyData> Dataset;
57 
58  vtkPoints* Points;
59  vtkFloatArray* Intensity;
60  vtkUnsignedIntArray* ScanLineId;
61  vtkFloatArray* Azimuth;
62  vtkFloatArray* SpindleAngle;
63  vtkFloatArray* Distance;
64  vtkFloatArray* ZHeight;
65  vtkFloatArray* ScanDelta;
66  vtkUnsignedIntArray* Timestamp;
67 };
68 
69 //----------------------------------------------------------------------------
70 vtkSmartPointer<vtkCellArray> NewVertexCells(vtkIdType numberOfVerts)
71 {
72  vtkSmartPointer<vtkIdTypeArray> cells = vtkSmartPointer<vtkIdTypeArray>::New();
73  cells->SetNumberOfValues(numberOfVerts*2);
74  vtkIdType* ids = cells->GetPointer(0);
75  for (vtkIdType i = 0; i < numberOfVerts; ++i)
76  {
77  ids[i*2] = 1;
78  ids[i*2+1] = i;
79  }
80 
81  vtkSmartPointer<vtkCellArray> cellArray = vtkSmartPointer<vtkCellArray>::New();
82  cellArray->SetCells(numberOfVerts, cells.GetPointer());
83  return cellArray;
84 }
85 
86 //-----------------------------------------------------------------------------
87 DataArrays CreateData(vtkIdType numberOfPoints)
88 {
89  vtkSmartPointer<vtkPolyData> polyData = vtkSmartPointer<vtkPolyData>::New();
90 
91  // points
92  vtkSmartPointer<vtkPoints> points = vtkSmartPointer<vtkPoints>::New();
93  points->SetDataTypeToFloat();
94  points->Allocate(numberOfPoints);
95  polyData->SetPoints(points.GetPointer());
96  polyData->SetVerts(NewVertexCells(numberOfPoints));
97 
98  // intensity
99  vtkSmartPointer<vtkFloatArray> intensity = vtkSmartPointer<vtkFloatArray>::New();
100  intensity->SetName("intensity");
101  intensity->Allocate(numberOfPoints);
102  polyData->GetPointData()->AddArray(intensity.GetPointer());
103 
104  // scan line id
105  vtkSmartPointer<vtkUnsignedIntArray> scanLineId = vtkSmartPointer<vtkUnsignedIntArray>::New();
106  scanLineId->SetName("scan_line_id");
107  scanLineId->Allocate(numberOfPoints);
108  polyData->GetPointData()->AddArray(scanLineId.GetPointer());
109 
110  // azimuth
111  vtkSmartPointer<vtkFloatArray> azimuth = vtkSmartPointer<vtkFloatArray>::New();
112  azimuth->SetName("azimuth");
113  azimuth->Allocate(numberOfPoints);
114  polyData->GetPointData()->AddArray(azimuth.GetPointer());
115 
116  // spindle angle
117  vtkSmartPointer<vtkFloatArray> spindleAngle = vtkSmartPointer<vtkFloatArray>::New();
118  spindleAngle->SetName("spindle_angle");
119  spindleAngle->Allocate(numberOfPoints);
120  polyData->GetPointData()->AddArray(spindleAngle.GetPointer());
121 
122  // range
123  vtkSmartPointer<vtkFloatArray> distance = vtkSmartPointer<vtkFloatArray>::New();
124  distance->SetName("distance");
125  distance->Allocate(numberOfPoints);
126  polyData->GetPointData()->AddArray(distance.GetPointer());
127 
128  // z
129  vtkSmartPointer<vtkFloatArray> zheight = vtkSmartPointer<vtkFloatArray>::New();
130  zheight->SetName("z");
131  zheight->Allocate(numberOfPoints);
132  polyData->GetPointData()->AddArray(zheight.GetPointer());
133 
134  // scan delta
135  vtkSmartPointer<vtkFloatArray> scanDelta = vtkSmartPointer<vtkFloatArray>::New();
136  scanDelta->SetName("scan_delta");
137  scanDelta->Allocate(numberOfPoints);
138  polyData->GetPointData()->AddArray(scanDelta.GetPointer());
139 
140  // timestamp
141  vtkSmartPointer<vtkUnsignedIntArray> timestamp = vtkSmartPointer<vtkUnsignedIntArray>::New();
142  timestamp->SetName("timestamp");
143  timestamp->Allocate(numberOfPoints);
144  polyData->GetPointData()->AddArray(timestamp.GetPointer());
145 
146  DataArrays arrays;
147  arrays.Dataset = polyData;
148  arrays.Points = points.GetPointer();
149  arrays.Intensity = intensity.GetPointer();
150  arrays.ScanLineId = scanLineId.GetPointer();
151  arrays.Azimuth = azimuth.GetPointer();
152  arrays.SpindleAngle = spindleAngle.GetPointer();
153  arrays.Distance = distance.GetPointer();
154  arrays.ZHeight = zheight.GetPointer();
155  arrays.ScanDelta = scanDelta.GetPointer();
156  arrays.Timestamp = timestamp.GetPointer();
157 
158  return arrays;
159 }
160 
161 
162 void AddScanLine(const ScanLineData& scanLine, DataArrays& dataArrays, double distanceRange[2], double edgeAngleThreshold, double heightRange[2])
163 {
164 
165  const bot_core::planar_lidar_t* msg = &scanLine.msg;
166 
167  double spindleAngle = scanLine.SpindleAngle;
168  unsigned int scanLineId = scanLine.ScanLineId;
169  double theta = msg->rad0;
170  const double thetaStep = msg->radstep;
171  const int numPoints = msg->nranges;
172  const std::vector<float>& ranges = msg->ranges;
173  const int numIntensities = msg->nintensities;
174  const std::vector<float>& intensities = msg->intensities;
175 
176  bool useIntensities = true;
177  if (numIntensities != numPoints){
178  useIntensities = false;
179  }
180 
181  if (numPoints < 2)
182  {
183  return;
184  }
185 
186  double t = 0.0;
187  const double tStep = 1.0/(numPoints-1);
188  Eigen::Quaterniond q0(scanLine.ScanToLocalStart.linear());
189  Eigen::Quaterniond q1(scanLine.ScanToLocalEnd.linear());
190  Eigen::Vector3d pos0(scanLine.ScanToLocalStart.translation());
191  Eigen::Vector3d pos1(scanLine.ScanToLocalEnd.translation());
192 
193  const bool doFilter = (edgeAngleThreshold > 0);
194  const float angleThresh = edgeAngleThreshold*M_PI/180;
195 
196  float prevDelta = 0;
197  float prevScanDelta = 0;
198  float prevRange = -1;
199  for (int i = 0; i < numPoints; ++i, t += tStep, theta += thetaStep)
200  {
201  if (ranges[i] < distanceRange[0] || ranges[i] > distanceRange[1])
202  {
203  continue;
204  }
205 
206 
207  // Edge effect filter
208  if ( (i > 0) && ( i+1 < numPoints) && doFilter)
209  {
210  float theta1 = msg->rad0 + (i-1)*msg->radstep;
211  float theta2 = msg->rad0 + i*msg->radstep;
212  float theta3 = msg->rad0 + (i+1)*msg->radstep;
213  float r1 = ranges[i-1];
214  float r2 = ranges[i];
215  float r3 = ranges[i+1];
216  Eigen::Vector2f p1(r1*std::cos(theta1), r1*std::sin(theta1));
217  Eigen::Vector2f p2(r2*std::cos(theta2), r2*std::sin(theta2));
218  Eigen::Vector2f p3(r3*std::cos(theta3), r3*std::sin(theta3));
219  Eigen::Vector2f pointDelta1 = (p1-p2).normalized();
220  Eigen::Vector2f pointDelta2 = (p3-p2).normalized();
221  Eigen::Vector2f ray = p2.normalized();
222  float angle1 = std::acos(ray.dot(pointDelta1));
223  if (angle1 > M_PI/2) angle1 = M_PI-angle1;
224  float angle2 = std::acos(ray.dot(pointDelta2));
225  if (angle2 > M_PI/2) angle2 = M_PI-angle2;
226  if ((angle1 < angleThresh) && (angle2 < angleThresh)) {
227  continue;
228  }
229  }
230 
231  float curRange = ranges[i];
232  float curDelta = (prevRange >= 0) ? (ranges[i] - prevRange) : 0;
233  prevScanDelta = (std::abs(prevDelta) > std::abs(curDelta)) ?
234  -prevDelta : curDelta;
235  prevDelta = curDelta;
236  prevRange = curRange;
237 
238  Eigen::Vector3d pt(ranges[i] * cos(theta), ranges[i] * sin(theta), 0.0);
239 
240  Eigen::Quaterniond q = q0.slerp(t, q1);
241  Eigen::Vector3d pos = (1-t)*pos0 + t*pos1;
242  pt = q*pt + pos;
243 
244  Eigen::Vector3d posBody(scanLine.BodyToLocalStart.translation());
245 
246  if ( (pt[2] < (posBody[2] + heightRange[0])) || (pt[2] > (posBody[2] + heightRange[1])) )
247  continue;
248 
249  dataArrays.Points->InsertNextPoint(pt[0], pt[1], pt[2]);
250 
251  if (useIntensities)
252  {
253  dataArrays.Intensity->InsertNextValue(intensities[i]);
254  }else
255  {
256  dataArrays.Intensity->InsertNextValue(0); // TODO: is it preferable NOT to insert a value?
257  }
258 
259  dataArrays.ScanLineId->InsertNextValue(scanLineId);
260  dataArrays.Azimuth->InsertNextValue(theta);
261  dataArrays.SpindleAngle->InsertNextValue(spindleAngle);
262  dataArrays.Distance->InsertNextValue(ranges[i]);
263  dataArrays.ZHeight->InsertNextValue(pt[2]);
264  dataArrays.ScanDelta->InsertNextValue(0);
265  int numValues = dataArrays.ScanDelta->GetNumberOfTuples();
266  if (numValues > 1) {
267  dataArrays.ScanDelta->SetValue(numValues-2, prevScanDelta);
268  }
269  dataArrays.Timestamp->InsertNextValue(msg->utime);
270  }
271 
272 }
273 
274 vtkSmartPointer<vtkPolyData> GetPointCloudFromScanLines(const std::vector<ScanLineData>& scanLines, double distanceRange[2], double edgeAngleThreshold, double heightRange[2])
275 {
276  //printf("GetPointCloud, given %d scan lines\n", scanLines.size());
277  DataArrays dataArrays = CreateData(800 * scanLines.size());
278 
279  for (size_t i = 0; i < scanLines.size(); ++i)
280  {
281  AddScanLine(scanLines[i], dataArrays, distanceRange, edgeAngleThreshold, heightRange);
282  }
283 
284  dataArrays.Dataset->SetVerts(NewVertexCells(dataArrays.Dataset->GetNumberOfPoints()));
285 
286  //printf("%.1f points per scan line\n", static_cast<float>(dataArrays.Dataset->GetNumberOfPoints())/scanLines.size());
287 
288  return dataArrays.Dataset;
289 }
290 
291 
292 } // end namespace
293 
294 
295 
296 #endif
t
float t
Definition: edl_shade.glsl:80
Generated on Fri Feb 10 2017 13:52:10 for Drake Designer by   doxygen 1.8.6