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

620 lines
18 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 "slam6d/scan_io_velodyne_frames.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
#ifdef _MSC_VER
#include "XGetopt.h"
#else
#include <getopt.h>
#endif
#define BLOCK_OFFSET 42+16
#define BLOCK_SIZE 1206
#define CIRCLELENGTH 260
#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;
struct velodyne_sample
{
float xyz[3]; // calibrated, projected into velodyne coordinate system
float distance;
float corredistance;
unsigned short rot;
float rotational;
BYTE intensity;
float theta,phi;
int offset;
float normal;
float normal_theta;
float smooth;
float scanline_drtZ;
float scanline_drtD;
float scanline_countZ;
float scanline_countD;
/////////////////////////Type of Points cloud/////////////////////////////////
int classType; // 0 nused , 1 road , 2 build , 3 Tree , 4 person ,5 car , 6 ground
};
long CountOfLidar = 0;
double velodyne_calibrated[VELODYNE_NUM_LASERS][5] =
{
//vertCorrection rotCorrection distCorrection vertOffsetCorrection horizOffsetCorrection
{ -7.158120, -4.954240, 0.000000 , 0.000000 , -4.000000 }, // laser 0
{ -6.817820, -2.814700, 0.000000 , 0.000000 , 4.000000 }, // laser 1
{ 0.317822, 2.814740, 0.000000 , 0.000000 , -4.000000 }, // laser 2
{ 0.658119, 4.954230, 0.000000 , 0.000000 , 4.000000 }, // laser 3
{ -6.477650, -0.679162, 0.000000 , 0.000000 , -4.000000 }, // laser 4
{ -6.137590, 1.455470, 0.000000 , 0.000000 , 4.000000 }, // laser 5
{ -8.520810, -1.455470, 0.000000 , 0.000000 , -4.000000 }, // laser 6
{ -8.179890, 0.679162, 0.000000 , 0.000000 , 4.000000 }, // laser 7
{ -5.797640, 3.592120, 0.000000 , 0.000000 , -4.000000 }, // laser 8
{ -5.457770, 5.733800, 0.000000 , 0.000000 , 4.000000 }, // laser 9
{ -7.839140, 2.814740, 0.000000 , 0.000000 , -4.000000 }, // laser 10
{ -7.498560, 4.954200, 0.000000 , 0.000000 , 4.000000 }, // laser 11
{ -3.080210, -4.954240, 0.000000 , 0.000000 , -4.000000 }, // laser 12
{ -2.740630, -2.814700, 0.000000 , 0.000000 , 4.000000 }, // laser 13
{ -5.117980, -5.733800, 0.000000 , 0.000000 , -4.000000 }, // laser 14
{ -4.778260, -3.592120, 0.000000 , 0.000000 , 4.000000 }, // laser 15
{ -2.401040, -0.679162, 0.000000 , 0.000000 , -4.000000 }, // laser 16
{ -2.061410, 1.455470, 0.000000 , 0.000000 , 4.000000 }, // laser 17
{ -4.438590, -1.455470, 0.000000 , 0.000000 , -4.000000 }, // laser 18
{ -4.098960, 0.679162, 0.000000 , 0.000000 , 4.000000 }, // laser 19
{ -1.721740, 3.592120, 0.000000 , 0.000000 , -4.000000 }, // laser 20
{ -1.382020, 5.733800, 0.000000 , 0.000000 , 4.000000 }, // laser 21
{ -3.759370, 2.814740, 0.000000 , 0.000000 , -4.000000 }, // laser 22
{ -3.419790, 4.954240, 0.000000 , 0.000000 , 4.000000 }, // laser 23
{ 0.998555, -4.954240, 0.000000 , 0.000000 , -4.000000 }, // laser 24
{ 1.339140, -2.814740, 0.000000 , 0.000000 , 4.000000 }, // laser 25
{ -1.042230, -5.733800, 0.000000 , 0.000000 , -4.000000 }, // laser 26
{ -0.702363, -3.592120, 0.000000 , 0.000000 , 4.000000 }, // laser 27
{ 1.679890, -0.679162, 0.000000 , 0.000000 , -4.000000 }, // laser 28
{ 2.020810, 1.455470, 0.000000 , 0.000000 , 4.000000 }, // laser 29
{ -0.362407, -1.455470, 0.000000 , 0.000000 , -4.000000 }, // laser 30
{ -0.022350, 0.679162, 0.000000 , 0.000000 , 4.000000 }, // laser 31
{ -22.737886, -7.443011, 0.000000 , 0.000000 , -4.000000 }, // laser 32
{ -22.226072, -4.224233, 0.000000 , 0.000000 , 4.000000 }, // laser 33
{ -11.513928, 4.224233, 0.000000 , 0.000000 , -4.000000 }, // laser 34
{ -11.002114, 7.443011, 0.000000 , 0.000000 , 4.000000 }, // laser 35
{ -21.714685, -1.018773, 0.000000 , 0.000000 , -4.000000 }, // laser 36
{ -21.203688, 2.183498, 0.000000 , 0.000000 , 4.000000 }, // laser 37
{ -24.790272, -2.183498, 0.000000 , 0.000000 , -4.000000 }, // laser 38
{ -24.276321, 1.018773, 0.000000 , 0.000000 , 4.000000 }, // laser 39
{ -20.693031, 5.3926148, 0.000000 , 0.000000 , -4.000000 }, // laser 40
{ -20.182682, 8.6188126, 0.000000 , 0.000000 , 4.000000 }, // laser 41
{ -23.762968, 4.2242332, 0.000000 , 0.000000 , -4.000000 }, // laser 42
{ -23.250172, 7.4430108, 0.000000 , 0.000000 , 4.000000 }, // laser 43
{ -16.615318, -7.4430108, 0.000000 , 0.000000 , -4.000000 }, // laser 44
{ -16.105938, -4.2242332, 0.000000 , 0.000000 , 4.000000 }, // laser 45
{ -19.672594, -8.6188126, 0.000000 , 0.000000 , -4.000000 }, // laser 46
{ -19.162729, -5.3926148, 0.000000 , 0.000000 , 4.000000 }, // laser 47
{ -15.596496, -1.018773, 0.000000 , 0.000000 , -4.000000 }, // laser 48
{ -15.086954, 2.1834979, 0.000000 , 0.000000 , 4.000000 }, // laser 49
{ -18.653046, -2.1834979, 0.000000 , 0.000000 , -4.000000 }, // laser 50
{ -18.143503, 1.018773, 0.000000 , 0.000000 , 4.000000 }, // laser 51
{ -14.577271, 5.3926148, 0.000000 , 0.000000 , -4.000000 }, // laser 52
{ -14.067405, 8.6188126, 0.000000 , 0.000000 , 4.000000 }, // laser 53
{ -17.634062, 4.2242332, 0.000000 , 0.000000 , -4.000000 }, // laser 54
{ -17.124681, 7.4430108, 0.000000 , 0.000000 , 4.000000 }, // laser 55
{ -10.489829, -7.4430108, 0.000000 , 0.000000 , -4.000000 }, // laser 56
{ -9.9770317, -4.2242332, 0.000000 , 0.000000 , 4.000000 }, // laser 57
{ -13.557318, -8.6188126, 0.000000 , 0.000000 , -4.000000 }, // laser 58
{ -13.046968, -5.3926148, 0.000000 , 0.000000 , 4.000000 }, // laser 59
{ -9.4636793, -1.018773, 0.000000 , 0.000000 , -4.000000 }, // laser 60
{ -8.949728, 2.1834979, 0.000000 , 0.000000 , 4.000000 }, // laser 61
{ -12.536313, -2.1834979, 0.000000 , 0.000000 , -4.000000 }, // laser 62
{ -12.025314, 1.018773, 0.000000 , 0.000000 , 4.000000 }, // laser 63
};
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 = 0;
BYTE buf[BLOCK_SIZE];
Point point;
BYTE *p;
unsigned short *ps;
unsigned short *pshort;
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;
double rotational;
double distance;
double corredistance;
int intensity;
// int physical;
int size;
//unsigned short rot;
double x, y, z;
/*
int circle_col = 0;
int circle_row = 0;
int circle_col_other = 0;
int circle_row_other = 0;
*/
for ( c = 0 ; c < CIRCLELENGTH; c++ )
{
fseek(fp , BLOCK_OFFSET, SEEK_CUR);
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 = ( ( double ) ( *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;
circle_col_other = c * 6 + i / 2; /////????
circle_row_other = logicalNO / 2;
*/
pt = ( short * ) ( p + 4 + j * 3 );
distance = absf ( ( *pt ) * 0.002 );
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] );
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;
x -= horizdffsetCorrection[physicalNO] * cos_ctheta;
y -= horizdffsetCorrection[physicalNO] * sin_ctheta;
point.x=y;
point.y=z;
point.z=x;
point.x *= -100;
point.y *= 100;
point.z *= 100;
point.reflectance=intensity/256.0;
if ((maxDist == -1 || sqr(point.x) + sqr(point.y) + sqr(point.z) < maxDist2*1.0)
&& (minDist == -1 || sqr(point.x) + sqr(point.y) + sqr(point.z) > minDist2*1.0))
{
ptss.push_back(point);
//printf("%f %f %f %f %f %f\n",point.x,point.y,point.z,point.reflectance,minDist2*1.0,minDist2*1.0);
}
}
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_frame::readScans(int start, int end, string &dir, int maxDist, int minDist,
double *euler, vector<Point> &ptss)
{
static int fileCounter = start;
string scanFileName;
string poseFileName;
string framesFileName;
FILE *scan_in = 0;
FILE *pose_in = 0;
ifstream frames_in;
// int my_fileNr = fileCounter;
double transMat[16];
int type;
if (end > -1 && fileCounter > end) return -1; // 'nuf read
scanFileName = dir + "scan" + ".bin";
poseFileName = dir + "scan" + ".pose";
framesFileName = dir + "scan" + to_string(fileCounter,3) + ".frames";
scan_in = fopen(scanFileName.c_str(),"rb");
if(scan_in==NULL)
{
cerr << "ERROR: Missing file " << scanFileName <<" "<<strerror(errno)<< endl; exit(1);
return 0;
}
//pose_in = fopen(poseFileName.c_str(),"rb");
if(pose_in==NULL)
{
//cout << "no file " << poseFileName <<" "<<strerror(errno)<< endl; ;
}
else
{
// cout<<"we get pose info"<<endl;
}
frames_in.open(framesFileName.c_str());
// read 3D scan
if (!frames_in.good()) { cerr << "ERROR: Missing file " << framesFileName << endl; exit(1); }
while(frames_in) {
try {
frames_in >> transMat >> type;
}
catch(const exception &e) {
break;
}
}
Matrix4ToEuler(transMat, &euler[3], euler);
cout << " @ pose (" << euler[0] << "," << euler[1] << "," << euler[2]
<< "," << euler[3] << "," << euler[4] << "," << euler[5] << ")" << endl;
velodyne_calib_precompute();
cout << "Processing Scan " << scanFileName;
cout.flush();
#if 0
raw_packet_t raw_tmp;
fseeko(scan_in, 0, SEEK_SET);
fread(&raw_tmp,sizeof(raw_packet_t),1,scan_in);
int revolution=raw_tmp.revolution+fileCounter+1;
int i=0;
while(raw_tmp.revolution!=revolution)
{
printf("%d %d\n",i++,raw_tmp.revolution);
fread(&raw_tmp,sizeof(raw_packet_t),1,scan_in);
}
#else
cout.flush();
ptss.reserve(12*32*CIRCLELENGTH);
#if 0
fseeko(scan_in, 0, SEEK_SET);
fseeko(scan_in, BLOCK_SIZE*CIRCLELENGTH*fileCounter, SEEK_CUR);
#else
fseek(scan_in, 24, SEEK_SET);
fseek(scan_in, (BLOCK_SIZE+BLOCK_OFFSET)*CIRCLELENGTH*fileCounter, SEEK_CUR);
#endif
#endif
read_one_packet(scan_in, ptss, maxDist, minDist);
cout << " with " << ptss.size() << " Points";
cout << " done " << fileCounter<<endl;
if(pose_in)
{
double poseinfo[6];
fseek(pose_in, 0, SEEK_SET);
fseek(pose_in, sizeof(double)*6*fileCounter, SEEK_CUR);
fread(poseinfo,sizeof(double)*6,1,pose_in);
euler[0] = poseinfo[0]*100 - 135701 ;
euler[1] = poseinfo[1]*100- 842.154;
euler[2] = poseinfo[2]*100+93003.5;
euler[3] = poseinfo[3];
euler[4] = poseinfo[4];
euler[5] = poseinfo[5];
printf("%f %f %f %f %f %f\n",poseinfo[0],poseinfo[1],poseinfo[2],poseinfo[3],poseinfo[4],poseinfo[5]);
fclose(pose_in);
}
fclose(scan_in);
fileCounter++;
frames_in.close();
frames_in.clear();
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_frame;
}
/**
* 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