-
-
Notifications
You must be signed in to change notification settings - Fork 21
/
main.cpp
85 lines (70 loc) · 2.78 KB
/
main.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
#include <iostream>
#include <vector>
#include <cmath>
#include <opencv2/opencv.hpp>
#include <opencv2/viz.hpp>
// Include VelodyneCapture Header
#include "VelodyneCapture.h"
int main( int argc, char* argv[] )
{
// Open VelodyneCapture that retrieve from Sensor
const boost::asio::ip::address address = boost::asio::ip::address::from_string( "192.168.1.21" );
const unsigned short port = 2368;
velodyne::VLP16Capture capture( address, port );
//velodyne::HDL32ECapture capture( address, port );
/*
// Open VelodyneCapture that retrieve from PCAP
const std::string filename = "../file.pcap";
velodyne::VLP16Capture capture( filename );
//velodyne::HDL32ECapture capture( filename );
*/
if( !capture.isOpen() ){
std::cerr << "Can't open VelodyneCapture." << std::endl;
return -1;
}
// Create Viewer
cv::viz::Viz3d viewer( "Velodyne" );
// Register Keyboard Callback
viewer.registerKeyboardCallback(
[]( const cv::viz::KeyboardEvent& event, void* cookie ){
// Close Viewer
if( event.code == 'q' && event.action == cv::viz::KeyboardEvent::Action::KEY_DOWN ){
static_cast<cv::viz::Viz3d*>( cookie )->close();
}
}
, &viewer
);
while( capture.isRun() && !viewer.wasStopped() ){
// Capture One Rotation Data
std::vector<velodyne::Laser> lasers;
capture >> lasers;
if( lasers.empty() ){
continue;
}
// Convert to 3-dimention Coordinates
std::vector<cv::Vec3f> buffer( lasers.size() );
for( const velodyne::Laser& laser : lasers ){
const double distance = static_cast<double>( laser.distance );
const double azimuth = laser.azimuth * CV_PI / 180.0;
const double vertical = laser.vertical * CV_PI / 180.0;
float x = static_cast<float>( ( distance * std::cos( vertical ) ) * std::sin( azimuth ) );
float y = static_cast<float>( ( distance * std::cos( vertical ) ) * std::cos( azimuth ) );
float z = static_cast<float>( ( distance * std::sin( vertical ) ) );
if( x == 0.0f && y == 0.0f && z == 0.0f ){
x = std::numeric_limits<float>::quiet_NaN();
y = std::numeric_limits<float>::quiet_NaN();
z = std::numeric_limits<float>::quiet_NaN();
}
buffer.push_back( cv::Vec3f( x, y, z ) );
}
// Create Widget
cv::Mat cloudMat = cv::Mat( static_cast<int>( buffer.size() ), 1, CV_32FC3, &buffer[0] );
cv::viz::WCloud cloud( cloudMat );
// Show Point Cloud
viewer.showWidget( "Cloud", cloud );
viewer.spinOnce();
}
// Close All Viewers
cv::viz::unregisterAllWindows();
return 0;
}