3dpcp/.svn/pristine/ee/eec5c24dec45440fb6660c4a921ce78cec84482e.svn-base
2012-09-16 14:33:11 +02:00

463 lines
13 KiB
Text
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/**
* @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;
// 我们借到的雷达的标定数据
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??×?±ê/////////////////////
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