3dpcp/.svn/pristine/5b/5b6818356d295bdacd23f82f454bd7bf0fc53d8c.svn-base
2012-10-05 08:55:14 +02:00

615 lines
17 KiB
Text
Raw Blame History

/*
* scan_io_velodyne implementation
*
* Copyright (C) Mohammad Faisal, Dorit Borrmann, Andreas Nuechter
*
* Released under the GPL version 3.
*
*/
/**
* @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.
* @author Mohammad Faisal. Smart Systems Group, Jacobs University Bremen gGmbH, Germany.
*/
#include "slam6d/point.h"
#include "veloslam/velodefs.h"
#include "scanio/scan_io_velodyne.h"
#include "slam6d/globals.icc"
#include <fstream>
#include<strstream>
#include <iostream>
using std::ifstream;
using std::ostrstream;
using std::cerr;
using std::endl;
using std::ends;
#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
#include <boost/filesystem/operations.hpp>
#include <boost/filesystem/fstream.hpp>
using namespace boost::filesystem;
#define DATA_PATH_PREFIX "scan"
#define DATA_PATH_SUFFIX ".bin"
#define POSE_PATH_PREFIX "scan"
#define POSE_PATH_SUFFIX ".pose"
#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;
//Changes the variable to be filled dynamically. Added a 6th variable to enable or disable a specific sensor
double velodyne_calibrated[VELODYNE_NUM_LASERS][6];
/**
* Method to fill the calibration data using the Velodyne64 calibration data from Wuhan.
*/
int backup()
{
double velodyne_calibrated1[64][6] =
{
{ -7.1581192, -4.5, 102, 21.560343, -2.5999999, 1},
{ -6.8178215, -3.4000001, 125, 21.516994, 2.5999999, 1},
{ 0.31782165, 3, 130, 20.617426, -2.5999999, 1},
{ 0.65811908, 4.5999999, 128, 20.574717, 2.5999999, 1},
{ -6.4776502, -0.5, 112, 21.473722, -2.5999999, 1},
{ -6.1375928, 1, 125, 21.430525, 2.5999999, 1},
{ -8.520812, -1.5, 106, 21.734608, -2.5999999, 1},
{ -8.1798887, 0.40000001, 127, 21.690901, 2.5999999, 1},
{ -5.797637, 4, 111, 21.387396, -2.5999999, 1},
{ -5.4577708, 5.5, 126, 21.34433, 2.5999999, 1},
{ -7.8391404, 3.0999999, 113, 21.647291, -2.5999999, 1},
{ -7.4985547, 4.5, 123, 21.603773, 2.5999999, 1},
{ -3.0802133, -4.5, 105, 21.044245, -2.5999999, 1},
{ -2.7406337, -3.2, 133, 21.001518, 2.5999999, 1},
{ -5.1179824, -5.5, 110, 21.301321, -2.5999999, 1},
{ -4.7782598, -4, 129, 21.258366, 2.5999999, 1},
{ -2.4010365, -0.2, 111, 20.958813, -2.5999999, 1},
{ -2.0614092, 1, 130, 20.916126, 2.5999999, 1},
{ -4.4385905, -1.2, 115, 21.215462, -2.5999999, 1},
{ -4.0989642, 0, 133, 21.172602, 2.5999999, 1},
{ -1.7217404, 3.8, 113, 20.873451, -2.5999999, 1},
{ -1.3820176, 5, 130, 20.830786, 2.5999999, 1},
{ -3.7593663, 3, 117, 21.129782, -2.5999999, 1},
{ -3.4197867, 4.5, 129, 21.086998, 2.5999999, 1},
{ 0.998555, -4.5, 107, 20.531982, -2.5999999, 1},
{ 1.339141, -3.2, 131, 20.489222, 2.5999999, 1},
{ -1.0422293, -5.4000001, 128, 20.788124, -2.5999999, 1},
{ -0.70236301, -4, 134, 20.745461, 2.5999999, 1},
{ 1.679889, -0.5, 124, 20.446428, -2.5999999, 1},
{ 2.0208123, 1, 136, 20.403601, 2.5999999, 1},
{ -0.36240739, -1.5, 131, 20.702793, -2.5999999, 1},
{ -0.022349782, 0.2, 136, 20.660116, 2.5999999, 1},
{ -22.737886, -7.8000002, 101, 16.019152, -2.5999999, 1},
{ -22.226072, -5, 88, 15.954137, 2.5999999, 1},
{ -11.513928, 4.5, 121, 14.680806, -2.5999999, 1},
{ -11.002114, 7.4000001, 88, 14.623099, 2.5999999, 1},
{ -21.714685, -1, 94, 15.889649, -2.5999999, 1},
{ -21.203688, 2, 88, 15.82566, 2.5999999, 1},
{ -24.790272, -2.5, 114, 16.284933, -2.5999999, 1},
{ -24.276321, 0.5, 89, 16.217583, 2.5999999, 1},
{ -20.693031, 6, 98, 15.762167, -2.5999999, 1},
{ -20.182682, 9, 92, 15.699132, 2.5999999, 1},
{ -23.762968, 4.5, 107, 16.15085, -2.5999999, 1},
{ -23.250172, 7.5, 80, 16.084715, 2.5999999, 1},
{ -16.615318, -7.5, 121, 15.26925, -2.5999999, 1},
{ -16.105938, -5, 92, 15.209245, 2.5999999, 1},
{ -19.672594, -9, 119, 15.63654, -2.5999999, 1},
{ -19.162729, -6, 89, 15.574372, 2.5999999, 1},
{ -15.596496, -1, 109, 15.14954, -2.5999999, 1},
{ -15.086954, 2, 88, 15.090119, 2.5999999, 1},
{ -18.653046, -2, 117, 15.51261, -2.5999999, 1},
{ -18.143503, 0.69999999, 88, 15.451235, 2.5999999, 1},
{ -14.577271, 5.5, 112, 15.030966, -2.5999999, 1},
{ -14.067405, 8.3999996, 87, 14.972065, 2.5999999, 1},
{ -17.634062, 5, 119, 15.390228, -6.1999998, 1},
{ -17.124681, 7.5, 97, 15.329572, 2.5999999, 1},
{ -10.489829, -7.5, 119, 14.565539, -2.5999999, 1},
{ -9.9770317, -4.6999998, 95, 14.508112, 2.5999999, 1},
{ -13.557318, -8.5, 126, 14.913401, -2.5999999, 1},
{ -13.046968, -6, 92, 14.854958, 2.5999999, 1},
{ -9.4636793, -1, 112, 14.450804, -2.5999999, 1},
{ -8.949728, 1.5, 93, 14.3936, 2.5999999, 1},
{ -12.536313, -2, 121, 14.796721, -2.5999999, 1},
{ -12.025314, 0.40000001, 96, 14.738676, 2.5999999, 1},
};
for ( int i = 0; i < VELODYNE_NUM_LASERS; i++ )
{
velodyne_calibrated[i][0] = velodyne_calibrated1[i][0];
velodyne_calibrated[i][1] = velodyne_calibrated1[i][1];
velodyne_calibrated[i][2] = velodyne_calibrated1[i][2];
velodyne_calibrated[i][3] = velodyne_calibrated1[i][3];
velodyne_calibrated[i][4] = velodyne_calibrated1[i][4];
velodyne_calibrated[i][5] = velodyne_calibrated1[i][5];
}
return 0;
}
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];
double enabled[VELODYNE_NUM_LASERS]; //New variable to change enabling and disabling of data.
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 enable
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;
enabled[i] = velodyne_calibrated[i][5];
}
return 0;
}
int calibrate(string filename)
{
int linecount = 0;
string line;
std::ifstream myfile (filename.c_str());
if (myfile.is_open())
{
cout << "Using Calibration File"<<endl;
string data[6];
getline (myfile,line);
string theDelimiter = ",";
int j=0;
do
{
getline (myfile,line);
int i = 0;
size_t start = 0, end = 0;
while ( end != string::npos && i < 6)
{
end = line.find( theDelimiter, start);
data[i] = line.substr(start, (end == string::npos)?string::npos:end - start);
start = ((end>(string::npos - theDelimiter.size()))?string::npos:end + theDelimiter.size());
i++;
}
velodyne_calibrated[j][0] = atof(data[0].c_str());
velodyne_calibrated[j][1] = atof(data[1].c_str());
velodyne_calibrated[j][2] = atof(data[2].c_str());
velodyne_calibrated[j][3] = atof(data[3].c_str());
velodyne_calibrated[j][4] = atof(data[4].c_str());
velodyne_calibrated[j][5] = atof(data[5].c_str());
linecount++;
j++;
}
while(!myfile.eof());
myfile.close();
if (linecount < 60)
{
for (int i = 32; i<64; i++)
{
velodyne_calibrated[i][0] = 0.0;
velodyne_calibrated[i][1] = 0.0;
velodyne_calibrated[i][2] = 0.0;
velodyne_calibrated[i][3] = 0.0;
velodyne_calibrated[i][4] = 0.0;
velodyne_calibrated[i][5] = 0.0;
}
}
}
else
{
cout << "Unable to open calibration file.\nUsing Wuhan hardcored values."<<endl;
backup();
}
return 0;
}
int read_one_packet (
FILE *fp,
PointFilter& filter,
std::vector<double>* xyz,
std::vector<unsigned char>* rgb,
std::vector<float>* reflectance,
std::vector<float>* amplitude,
std::vector<int>* type,
std::vector<float>* deviation)
{
int c, i, j;
unsigned char Head;
BYTE buf[BLOCK_SIZE];
Point point;
BYTE *p;
unsigned short *ps;
unsigned short *pt;
unsigned short *pshort;
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 < 120 && 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_GROUND;
point.x=x*100;
point.y=z*100;
point.z=-y*100;
double p[3];
p[0] = point.x;
p[1] = point.y;
p[2] = point.z;
//Enable Check. Could be moved earlier to remove all the unnecessary calculations.
//Here to simplify the implementation
if(filter.check(p) && (enabled[physicalNO]==1))
{
for(int ii = 0; ii < 3; ++ii) xyz->push_back(p[ii]);
}
}
}
p = p + 100;
ps = ( unsigned short * ) p;
}
}
return 0;
}
std::list<std::string> ScanIO_velodyne::readDirectory(const char* dir_path, unsigned int start, unsigned int end)
{
//Calling the calibration method with the file name "calibration.txt" in the same folder as the scan.bin
string califilename;
string dir(dir_path);
califilename = dir + "calibration" + ".txt";
calibrate(califilename);
std::list<std::string> identifiers;
// fileCounter = start;
velodyne_calib_precompute();
for(unsigned int i = start; i <= end; ++i)
{
std::string identifier(to_string(i,3));
path data(dir_path);
data /= path(std::string(DATA_PATH_PREFIX) + DATA_PATH_SUFFIX);
path pose(dir_path);
pose /= path(std::string(POSE_PATH_PREFIX) + identifier + POSE_PATH_SUFFIX);
if(!exists(data))
break;
identifiers.push_back(identifier);
}
return identifiers;
}
void ScanIO_velodyne::readPose(const char* dir_path, const char* identifier, double* pose)
{
unsigned int i;
// on pose information for veloslam
for(i = 0; i < 6; ++i) pose[i] = 0.0;
for(i = 3; i < 6; ++i) pose[i] = 0.0;
return;
}
bool ScanIO_velodyne::supports(IODataType type)
{
return !!(type & (DATA_XYZ));
}
void ScanIO_velodyne::readScan(
const char* dir_path,
const char* identifier,
PointFilter& filter,
std::vector<double>* xyz,
std::vector<unsigned char>* rgb,
std::vector<float>* reflectance,
std::vector<float>* temperature,
std::vector<float>* amplitude,
std::vector<int>* type,
std::vector<float>* deviation)
{
unsigned int i;
FILE *scan_in;
path data_path(dir_path);
data_path /= path(std::string(DATA_PATH_PREFIX) + DATA_PATH_SUFFIX);
if(!exists(data_path))
throw std::runtime_error(std::string("There is no scan file for [") + identifier + "] in [" + dir_path + "]");
char filename[256];
sprintf(filename, "%s%s%s",dir_path ,DATA_PATH_PREFIX, DATA_PATH_SUFFIX );
#ifdef _MSC_VER
scan_in = fopen(filename,"rb");
#else
scan_in = fopen64(filename,"rb");
#endif
if(scan_in == NULL)
{
cerr<<data_path <<endl;
cerr << "ERROR: Missing file " << data_path <<" "<<strerror(errno)<< endl;
exit(1);
return;
}
cout << "Processing Scan " << data_path;
cout.flush();
xyz->reserve(12*32*CIRCLELENGTH);
fileCounter= atoi(identifier);
#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,
filter,
xyz,
rgb,
reflectance,
amplitude,
type,
deviation);
cout << " with " << xyz->size() << " Points";
cout << " done " << fileCounter<<endl;
fclose(scan_in);
// process next frames
}
/**
* 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