josch/3dpcp
1
0
Fork 0
Code Issues Pull requests Projects Releases Wiki Activity
3dpcp/.svn/pristine/ee/eec5c24dec45440fb6660c4a921ce78cec84482e.svn-base

464 lines
13 KiB
Text
Raw Normal View History

initial commit
2012-09-16 12:33:11 +00:00
/**
* @file
* @brief Implementation of reading 3D scans
* @author Andreas Nuechter. Jacobs University Bremen gGmbH
* @author Dorit Borrmann. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
*/
#include "veloslam/velodefs.h"
#include "slam6d/scan_io_velodyne.h"
#include "slam6d/globals.icc"
#include <fstream>
using std::ifstream;
#include <iostream>
using std::cerr;
using std::endl;
#include <algorithm>
using std::swap;
#include<cstdio>
#include<cstdlib>
#include<cmath>
#include<cstring>
#include <errno.h>
using namespace std;
#ifdef _MSC_VER
#include <windows.h>
#endif
#define BLOCK_OFFSET 42+16
#define BLOCK_SIZE 1206
#define CIRCLELENGTH 360
#define VELODYNE_NUM_LASERS 64
#define CircleBufferSize CIRCLELENGTH*32*12
#define CIRCLEROUND CIRCLELENGTH*6
#define RADIANS_PER_LSB 0.0174532925
#define METERS_PER_LSB 0.002
#define METERS_PER_CM 0.01
#define TWOPI_INV (0.5/M_PI)
#define TWOPI (2*M_PI)
typedef struct raw_packet
{
unsigned char dat[1200];
unsigned short revolution;
unsigned char status[4];
} raw_packet_t;
typedef unsigned char BYTE;
long CountOfLidar = 0;
// <20><><EFBFBD>ǽ赽<C7BD><E8B5BD><EFBFBD>״<EFBFBD><D7B4>ı궨<C4B1><EAB6A8><EFBFBD><EFBFBD>
double velodyne_calibrated[VELODYNE_NUM_LASERS][5] =
{
{ -7.1581192, -4.5, 102, 21.560343, -2.5999999},
{ -6.8178215, -3.4000001, 125, 21.516994, 2.5999999},
{ 0.31782165, 3, 130, 20.617426, -2.5999999},
{ 0.65811908, 4.5999999, 128, 20.574717, 2.5999999},
{ -6.4776502, -0.5, 112, 21.473722, -2.5999999},
{ -6.1375928, 1, 125, 21.430525, 2.5999999},
{ -8.520812, -1.5, 106, 21.734608, -2.5999999},
{ -8.1798887, 0.40000001, 127, 21.690901, 2.5999999},
{ -5.797637, 4, 111, 21.387396, -2.5999999},
{ -5.4577708, 5.5, 126, 21.34433, 2.5999999},
{ -7.8391404, 3.0999999, 113, 21.647291, -2.5999999},
{ -7.4985547, 4.5, 123, 21.603773, 2.5999999},
{ -3.0802133, -4.5, 105, 21.044245, -2.5999999},
{ -2.7406337, -3.2, 133, 21.001518, 2.5999999},
{ -5.1179824, -5.5, 110, 21.301321, -2.5999999},
{ -4.7782598, -4, 129, 21.258366, 2.5999999},
{ -2.4010365, -0.2, 111, 20.958813, -2.5999999},
{ -2.0614092, 1, 130, 20.916126, 2.5999999},
{ -4.4385905, -1.2, 115, 21.215462, -2.5999999},
{ -4.0989642, 0, 133, 21.172602, 2.5999999},
{ -1.7217404, 3.8, 113, 20.873451, -2.5999999},
{ -1.3820176, 5, 130, 20.830786, 2.5999999},
{ -3.7593663, 3, 117, 21.129782, -2.5999999},
{ -3.4197867, 4.5, 129, 21.086998, 2.5999999},
{ 0.998555, -4.5, 107, 20.531982, -2.5999999},
{ 1.339141, -3.2, 131, 20.489222, 2.5999999},
{ -1.0422293, -5.4000001, 128, 20.788124, -2.5999999},
{ -0.70236301, -4, 134, 20.745461, 2.5999999},
{ 1.679889, -0.5, 124, 20.446428, -2.5999999},
{ 2.0208123, 1, 136, 20.403601, 2.5999999},
{ -0.36240739, -1.5, 131, 20.702793, -2.5999999},
{ -0.022349782, 0.2, 136, 20.660116, 2.5999999},
{ -22.737886, -7.8000002, 101, 16.019152, -2.5999999},
{ -22.226072, -5, 88, 15.954137, 2.5999999},
{ -11.513928, 4.5, 121, 14.680806, -2.5999999},
{ -11.002114, 7.4000001, 88, 14.623099, 2.5999999},
{ -21.714685, -1, 94, 15.889649, -2.5999999},
{ -21.203688, 2, 88, 15.82566, 2.5999999},
{ -24.790272, -2.5, 114, 16.284933, -2.5999999},
{ -24.276321, 0.5, 89, 16.217583, 2.5999999},
{ -20.693031, 6, 98, 15.762167, -2.5999999},
{ -20.182682, 9, 92, 15.699132, 2.5999999},
{ -23.762968, 4.5, 107, 16.15085, -2.5999999},
{ -23.250172, 7.5, 80, 16.084715, 2.5999999},
{ -16.615318, -7.5, 121, 15.26925, -2.5999999},
{ -16.105938, -5, 92, 15.209245, 2.5999999},
{ -19.672594, -9, 119, 15.63654, -2.5999999},
{ -19.162729, -6, 89, 15.574372, 2.5999999},
{ -15.596496, -1, 109, 15.14954, -2.5999999},
{ -15.086954, 2, 88, 15.090119, 2.5999999},
{ -18.653046, -2, 117, 15.51261, -2.5999999},
{ -18.143503, 0.69999999, 88, 15.451235, 2.5999999},
{ -14.577271, 5.5, 112, 15.030966, -2.5999999},
{ -14.067405, 8.3999996, 87, 14.972065, 2.5999999},
{ -17.634062, 5, 119, 15.390228, -6.1999998},
{ -17.124681, 7.5, 97, 15.329572, 2.5999999},
{ -10.489829, -7.5, 119, 14.565539, -2.5999999},
{ -9.9770317, -4.6999998, 95, 14.508112, 2.5999999},
{ -13.557318, -8.5, 126, 14.913401, -2.5999999},
{ -13.046968, -6, 92, 14.854958, 2.5999999},
{ -9.4636793, -1, 112, 14.450804, -2.5999999},
{ -8.949728, 1.5, 93, 14.3936, 2.5999999},
{ -12.536313, -2, 121, 14.796721, -2.5999999},
{ -12.025314, 0.40000001, 96, 14.738676, 2.5999999},
};
double rotCorrection[VELODYNE_NUM_LASERS];
double vertCorrection[VELODYNE_NUM_LASERS];
double distCorrection[VELODYNE_NUM_LASERS];
double vertoffsetCorrection[VELODYNE_NUM_LASERS];
double horizdffsetCorrection[VELODYNE_NUM_LASERS];
int physical2logical[VELODYNE_NUM_LASERS];
int logical2physical[VELODYNE_NUM_LASERS];
double absf ( double a )
{
if ( a < 0 )
return -a;
return a;
}
int velodyne_physical_to_logical ( int phys )
{
return physical2logical[phys];
}
int velodyne_logical_to_physical ( int logical )
{
return logical2physical[logical];
}
int laser_phi_compare(const void *_a, const void *_b)
{
int a = *((int*) _a);
int b = *((int*) _b);
if (velodyne_calibrated[a][0] < velodyne_calibrated[b][0])
return -1;
return 1;
}
/** valid only for v > 0 **/
static inline double mod2pi_positive(double vin)
{
double q = vin * TWOPI_INV + 0.5;
int qi = (int) q;
return vin - qi*TWOPI;
}
/** Map v to [-PI, PI] **/
static inline double mod2pi(double vin)
{
if (vin < 0)
return -mod2pi_positive(-vin);
else
return mod2pi_positive(vin);
}
/** Return vin such that it is within PI degrees of ref **/
static inline double mod2pi_ref(double ref, double vin)
{
return ref + mod2pi(vin - ref);
}
int velodyne_calib_precompute()
{
int i;
int logical;
for ( i = 0; i < VELODYNE_NUM_LASERS; i++ )
logical2physical[i] = i;
qsort ( logical2physical, VELODYNE_NUM_LASERS, sizeof ( int ), laser_phi_compare );
for ( logical = 0; logical < VELODYNE_NUM_LASERS; logical++ )
{
physical2logical[logical2physical[logical]] = logical;
}
//vertCorrection rotCorrection distCorrection vertOffsetCorrection horizOffsetCorrection
for ( i = 0; i < VELODYNE_NUM_LASERS; i++ )
{
vertCorrection[i] = velodyne_calibrated[i][0] * RADIANS_PER_LSB;
rotCorrection[i] = velodyne_calibrated[i][1] * RADIANS_PER_LSB;
distCorrection[i] = velodyne_calibrated[i][2] * METERS_PER_CM;
vertoffsetCorrection[i] = velodyne_calibrated[i][3] * METERS_PER_CM;
horizdffsetCorrection[i] = velodyne_calibrated[i][4] * METERS_PER_CM;
}
return 0;
}
int read_one_packet ( FILE *fp, vector<Point> &ptss, int maxDist, int minDist )
{
int maxDist2 = sqr(maxDist);
int minDist2 = sqr(minDist);
int c, i, j;
unsigned char Head;
BYTE buf[BLOCK_SIZE];
Point point;
BYTE *p;
unsigned short *ps;
unsigned short *pshort;
unsigned short *pt;
double ctheta;
double theta, phi;
double sin_ctheta, cos_ctheta;
double sin_theta, cos_theta;
double sin_phi, cos_phi;
unsigned short physicalNO;
unsigned short logicalNO;
float rotational;
float distance;
float corredistance;
int intensity;
int physical;
int size;
unsigned short rot;
double x, y, z;
int circle_col = 0;
int circle_row = 0;
for ( c = 0 ; c < CIRCLELENGTH; c++ )
{
#ifdef _MSC_VER
fseek(fp , BLOCK_OFFSET, SEEK_CUR);
#else
fseeko(fp , BLOCK_OFFSET, SEEK_CUR);
#endif
size=fread ( buf, 1, BLOCK_SIZE, fp );
if(size<BLOCK_SIZE)
return -1;
ps = ( unsigned short * ) buf;
p = buf;
physicalNO = 0;
for ( i = 0; i < 12; i++ )
{
//Each frame start with 0xEEFF || 0xDDFF
if ( *ps == 0xEEFF )
Head = 0;
else if ( *ps == 0xDDFF )
Head = 32;
pshort = ( unsigned short * ) ( p + 2 );
rot = ( ( unsigned short ) ( *pshort ) / 100.0 );
circle_col = c * 6 + i / 2;
rotational = ( ( float ) ( *pshort ) ) / 100.0;
for ( j = 0; j < 32; j++ )
{
physicalNO = j + Head;
logicalNO = velodyne_physical_to_logical ( physicalNO );
circle_col = c * 6 + i / 2;
circle_row = logicalNO;
pt = ( unsigned short * ) ( p + 4 + j * 3 );
distance = absf ( ( *pt ) * 0.002 );
if( distance < 60 && distance > 2.2 )
{
BYTE *inty = ( BYTE * ) ( p + 4 + j * 3 + 2 );
intensity = *inty;
ctheta = 2 * M_PI - rotational * RADIANS_PER_LSB;
if ( ctheta == 2*M_PI )
ctheta = 0;
sin_ctheta = sin ( ctheta );
cos_ctheta = cos ( ctheta );
//vertCorrection rotCorrection distCorrection vertOffsetCorrection horizOffsetCorrection
// corredistance = ( distance + distCorrection[physicalNO] ) * ( 1.0 + vertoffsetCorrection[physicalNO] );
corredistance = ( distance + distCorrection[physicalNO] ) ;
theta = mod2pi_ref ( M_PI, ctheta + rotCorrection[physicalNO] ); /////////????////////
phi = vertCorrection[physicalNO]; ////////?????/////////////////
sin_theta = sin ( theta );
cos_theta = cos ( theta );
sin_phi = sin ( phi );
cos_phi = cos ( phi );
/////////////////////?a??<3F><>?<3F><><EFBFBD><EFBFBD>/////////////////////
x = corredistance * cos_theta * cos_phi;
y = corredistance * sin_theta * cos_phi;
z = corredistance * sin_phi +vertoffsetCorrection[physicalNO]*cos_phi;
x -= horizdffsetCorrection[physicalNO] * cos_ctheta;
y -= horizdffsetCorrection[physicalNO] * sin_ctheta;
point.rad = sqrt( x*x + y*y );
point.tan_theta = y/x;
// point.type|=POINT_TYPE_INVALID;
point.type = POINT_TYPE_GROUND;
point.x=x*100;
point.y=z*100;
point.z=-y*100;
{
ptss.push_back(point);
}
}
}
p = p + 100;
ps = ( unsigned short * ) p;
}
}
return 0;
}
/**
* Reads specified scans from given directory in
* the file format Riegl Laser Measurement GmbH
* uses. It will be compiled as shared lib.
*
* Scan poses will NOT be initialized after a call
* to this function. Initial pose estimation works
* only with the -p switch, i.e., trusting the initial
* estimations by Riegl.
*
* The scans have to be exported from the Riegl software
* as follows:
* 1. Export point cloud data to ASCII
* Use Scanners own Coordinate System (SOCS)
* X Y Z Range Theta Phi Reflectance
* 2. Export acqusition location (after you have registered
* with the Riegl software)
* Export SOP
* Write out as .dat file
*
* @param start Starts to read with this scan
* @param end Stops with this scan
* @param dir The directory from which to read
* @param maxDist Reads only Points up to this Distance
* @param minDist Reads only Points from this Distance
* @param euler Initital pose estimates (will not be applied to the points
* @param ptss Vector containing the read points
*/
int ScanIO_velodyne::readScans(int start, int end, string &dir, int maxDist, int minDist,
double *euler, vector<Point> &ptss)
{
static int fileCounter = start;
string scanFileName;
FILE *scan_in;
if (end > -1 && fileCounter > end) return -1; // 'nuf read
scanFileName = dir + "scan" + ".bin";
#ifdef _MSC_VER
scan_in = fopen(scanFileName.c_str(),"rb");
#else
scan_in = fopen64(scanFileName.c_str(),"rb");
#endif
if(scan_in == NULL)
{
cerr<<scanFileName <<endl;
cerr << "ERROR: Missing file " << scanFileName <<" "<<strerror(errno)<< endl;
exit(1);
return 0;
}
velodyne_calib_precompute();
cout << "Processing Scan " << scanFileName;
cout.flush();
ptss.reserve(12*32*CIRCLELENGTH);
#ifdef _MSC_VER
fseek(scan_in, 24, SEEK_SET);
fseek(scan_in, (BLOCK_SIZE+BLOCK_OFFSET)*CIRCLELENGTH*fileCounter, SEEK_CUR);
#else
fseeko(scan_in, 24, SEEK_SET);
fseeko(scan_in, (BLOCK_SIZE+BLOCK_OFFSET)*CIRCLELENGTH*fileCounter, SEEK_CUR);
#endif
read_one_packet(scan_in, ptss, maxDist, minDist);
cout << " with " << ptss.size() << " Points";
cout << " done " << fileCounter<<endl;
fclose(scan_in);
fileCounter++;
return fileCounter-1;
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) ScanIO* create()
#else
extern "C" ScanIO* create()
#endif
{
return new ScanIO_velodyne;
}
/**
* class factory for object construction
*
* @return Pointer to new object
*/
#ifdef _MSC_VER
extern "C" __declspec(dllexport) void destroy(ScanIO *sio)
#else
extern "C" void destroy(ScanIO *sio)
#endif
{
delete sio;
}
#ifdef _MSC_VER
BOOL APIENTRY DllMain(HANDLE hModule, DWORD dwReason, LPVOID lpReserved)
{
return TRUE;
}
#endif
Reference in a new issue Copy permalink