Hey guys,
I have the same problem:
The compiler gives me the following notification after running:
terminate called after throwing an instance of ‘std::out_of_range’
what(): vector::_M_range_check
This is my code, it would be nice, if someone could help me.
cRawSensorData **ppData;
pMediaSample->Lock((const tVoid **)&ppData);
cRawSensorData* pRawSensorData = *ppData;
tUInt8 ui8DeltaPhi=GetPropertyInt(“Angular Increment”); //angular increment 1°
//We need an array with the known raypoints and information if a rayline is occupied with raypoints --> declare a vector of vectors of floats named raypoints
vector<vector > Raypoints; // right now empty
QHash<int,double> point;
vector<vector > Layer;
for(tUInt16 ui16Phi=0; ui16Phi<360; ui16Phi+=ui8DeltaPhi)
{
Raypoints.push_back(vector()); // Add an empty row
Layer.push_back(vector()); // Add an empty row
}
for(tUInt16 ui16Phi=0; ui16Phi<360; ui16Phi+=ui8DeltaPhi)
{
for(tUInt8 ui8Layer=0; ui8Layer<64; ++ui8Layer ){
tUInt32 ui32Index;
pRawSensorData->getIndex(ui32Index,ui8Layer,ui16Phi, 0);
point = pRawSensorData->m_vScan[ui32Index];
if(point.contains(propDist)){
Raypoints.at(ui16Phi).push_back(point[propDist]); // Add column to all rows
Layer.at(ui16Phi).push_back(ui8Layer); // Add column to all rows
}
}
}
vector<CvKalman*>pKalman; //Pointer with data type CvKalman, necessary for the creation of Kalman filters
vector<vector > LifeFilter;
vector<vector >Line; //Line Vector of vectors, all r-values are saved, index is both the line-number and the number of points within the line
//Kalman equations:
//x_k = F*x_k -1 + B*u_k + w_k
//z_k = H_k*x_k + v_k
//w_k and v_k are respectively Gaussian distributions with:
//w_k ~ N(0,Q_k)
//v_k ~ N(0,R_k)
//Initialize Kalman parameters --> Assumption: These parameters are time invariant
const int F[] = { 1, ui8DeltaPhi, 0, 1 }; //Transition matrix F
CvMat* pH = cvCreateMat( 1, 2, CV_32FC1 );
cvSetIdentity( pH, cvRealScalar(1) ); //The measurement matrix is H=[1,0]
//HDL64-E_Velodyne_Laser: We know R_k=sigma^2 of Velodyne
//From the manual: sigma=2cm=0.02m --> variance=sigma^2=0.02^2=R_k
CvMat* pRk = cvCreateMat( 1, 1, CV_32FC1 );
cvSetIdentity( pRk, cvRealScalar(GetPropertyFloat(“Variance Noise Velodyne”)) );
CvMat* pQk = cvCreateMat( 2, 2, CV_32FC1 ); //Process noise covarinace Q_k
cvSetIdentity( pQk, cvRealScalar(GetPropertyFloat(“Process Noise Covariance”)) );
//Variable in order to be able to count the “Filter amount”
tUInt32 ui32Enumerate = 0;
for(tUInt16 ui16Phi=0; ui16Phi<360; ui16Phi+=ui8DeltaPhi){
tUInt16 ui16RayNo = ui16Phi/ui8DeltaPhi+1; //Ray Number
vector<vector > Prediction(2, vector()); //Prediction matrix
vector<vector > PCov(2, vector()); //Prior Error Covariance matrix
CvMat* pChiThreshold = cvCreateMat( 1, 1, CV_32FC1 ); //Threshold for Mahalanobis distance
tFloat32 f32L = GetPropertyFloat(“Initial Value L for P”); // Necessary for Matrix P_k in the initial state
if( ( !( Raypoints.at(ui16Phi).empty() ) ) || ( Raypoints.at(ui16Phi).size() )>0 ){
//if( ui16RayNo > 0 ){
// LifeFilter.at(ui16RayNo).clear(); //Usually this row will be empty, just used for unforseeable/unpredictable occurrences
//}
if( ( (ui16RayNo-1) >= 0 ) && (!pKalman.empty() ) && ( !LifeFilter.at(ui16RayNo-1).empty()) ){
for(tUInt32 ui32Count=0; ui32Count < LifeFilter.at(ui16RayNo-1).size(); ++ui32Count){
//Prediction step of the state vector, NULL because no control matrix, xk’=Fxk-1+B*uk-1 with B=0
memcpy( (pKalman.at(LifeFilter.at(ui16RayNo-1).at(ui32Count)))->transition_matrix->data.fl, F, sizeof(F));
memcpy( (pKalman.at(LifeFilter.at(ui16RayNo-1).at(ui32Count)))->process_noise_cov->data.fl, pQk, sizeof(pQk));
//Kalman Prediction step, the (prior) state vector pointer is returned via function
cvKalmanPredict( (pKalman.at(LifeFilter.at(ui16RayNo-1).at(ui32Count) )), NULL);
//Auxiliary pointers to matrices are named pM1 - pM2 - … and so on
CvMat* pM1 = cvCreateMat( 2, 2, CV_32FC1 );
pM1 = ( (pKalman.at(LifeFilter.at(ui16RayNo-1).at(ui32Count)))->state_pre );
if( ui32Count == LifeFilter.at(ui16RayNo-1).size()-1){
Prediction.at(0).clear();
Prediction.at(1).clear();
PCov.at(0).clear();
PCov.at(1).clear();
}
Prediction.at(0).push_back( cvmGet( pM1, 0, 0 ) );
Prediction.at(1).push_back( cvmGet( pM1, 1, 0 ) );
//Compute value prior error covariance
CvMat* pM2 = cvCreateMat( 2, 2, CV_32FC1 );
pM2 = ( (pKalman.at(LifeFilter.at(ui16RayNo-1).at(ui32Count)))->error_cov_pre );
PCov.at(0).push_back( cvmGet( pM2, 0, 0 ) );
PCov.at(0).push_back( cvmGet( pM2, 0, 1 ) );
PCov.at(1).push_back( cvmGet( pM2, 1, 0 ) );
PCov.at(1).push_back( cvmGet( pM2, 1, 1 ) );
}
}
for(tUInt32 ui32=0; ui32 < Raypoints.at(ui16Phi).size(); ++ui32 ){
if( pKalman.empty() ){
tFloat64 f64R0 = Raypoints.at(ui16Phi).at(ui32);
//Creation of Kalman Filter
//cvCreateKalman(int dynam_params, int measure_params, int control_params) with
//dynam_params - dimensionality of the state vector
//measure_params - dimensionality of the measurement vector
//control_params - dimensionality of the control vector
pKalman.push_back( cvCreateKalman(2,1,0) ); //first pKalman pointer is created
cvSetIdentity( pKalman.at(0)->state_post, cvRealScalar(f64R0) );
cvSetIdentity( pKalman.at(0)->state_pre, cvRealScalar(0));
//Initial Post Covarinace Matrix
CvMat* P = cvCreateMat(2,2,CV_32FC1);
cvZero( P );
cvmSet( P, 0, 0, f32L );
cvmSet( P, 0, 1, 0 );
cvmSet( P, 1, 0, 0 );
cvmSet( P, 1, 1, f32L );
memcpy( pKalman.at(0)->error_cov_post->data.fl, P, sizeof§);
Line.at(0).push_back(f64R0);
continue;
}
//Filter has to be updated --> Computation of Mahalanobis distance is possible
//Determine the measurement residual v_k=z_k-H_k*x_k(-)=z_k-H*x_k(-) within the upcoming for-loop
CvMat *pV = cvCreateMat( 1, 2, CV_32FC1 );
cvZero( pV );
cvmSet( pV, 0, 0, -1 );
cvmSet( pV, 0, 1, 0 ); //pV has the same matrix structure as -H=[-1 0]=-H_k
tFloat32 f32MinValue = 10^10; //very large, unreachable distance as start Mahalanobis distance --> compute Minimum
tUInt32 ui32LineValue;
if( !( pKalman.size() ) ){
ui32LineValue = 0;
}
if( LifeFilter.at(ui16RayNo-1).empty() ){
ui32LineValue = 0;
}
for(tUInt32 ui32Temp=0; ui32Temp < LifeFilter.at(ui16RayNo-1).size(); ++ui32Temp ){
CvMat *pMeasurement = cvCreateMat( 1, 1, CV_64FC1 );
cvZero( pMeasurement );
cvmSet( pMeasurement, 0, 0, Raypoints.at(ui16Phi).at(ui32) );
CvMat* pM3 = cvCreateMat(2, 1 ,CV_32FC1);
cvmSet( pM3, 0 , ui32Temp , Prediction.at(0).at(ui32Temp) );
cvmSet( pM3, 1 , ui32Temp , Prediction.at(1).at(ui32Temp) );
cvMatMulAdd(pV, pM3 , pMeasurement, pV );
//pV is now a pointer to the measurement residual v_k --> required for Mahalanobis distance
//Determine the measurement prediction covariance S_k=H_k*P_k(-)*H_k(T)+R_k
tUInt32 ui32Var = 2*ui32Temp; // after every 2nd column of PCov new raypoint --> variaböe ui32Var is used to describe this feature
tUInt32 ui32Var1 = 2*ui32Temp+1;
CvMat* pM4 = cvCreateMat( 2, 2, CV_32FC1 );
cvmSet( pM4, 0 , ui32Temp , PCov.at(0).at(ui32Var) );
cvmSet( pM4, 0 , ui32Temp , PCov.at(0).at(ui32Var1) );
cvmSet( pM4, 1 , ui32Temp , PCov.at(1).at(ui32Var) );
cvmSet( pM4, 1 , ui32Temp , PCov.at(1).at(ui32Var1) );
CvMat* pM5 = cvCreateMat( 1, 2, CV_32FC1 );
cvMatMul( pH , pM4, pM5); //Compute H_k*P_k(-)
CvMat* pM6 = cvCreateMat( 2, 1, CV_32FC1 );
cvTranspose(pH, pM6 ); //Compute H_k(T) --> Tranpose H_k
CvMat* pS = cvCreateMat( 1, 1, CV_32FC1); //pointer to prediction covarinace S_k
cvMatMulAdd(pM5, pM6 , pRk, pS );
//Determine the Mahalanobis distance (see Bachelor thesis)
//v_k(T)*S_k(-1)*v_k
//Transposed matrix v_k(T): v_k(T)=v_k (because 1x1 matrix)
CvMat* pM7 = cvCreateMat( 1, 1, CV_32FC1);
cvTranspose( pV, pM7 );
CvMat* pM8 = cvCreateMat( 1, 1, CV_32FC1);
cvInvert( pS, pM8 );
CvMat* pM9 = cvCreateMat( 1, 1, CV_32FC1 );
cvMatMul( pM7, pM8, pM9); //pM9: pointer to the matrix product v_k(T)*S_k(-1) with S_k(-1)=1/S_k
CvMat* pMDist = cvCreateMat( 1, 1, CV_32FC1);
cvMatMul( pM9, pV , pMDist); //pMDist: pointer to the Mahalanobis distance v_k(T)*S_k(-1)*v_k
//if-construction --> necessary for the minimum distance
if( f32MinValue > *(pMDist->data.fl) ){
f32MinValue = *(pMDist->data.fl);
ui32LineValue = LifeFilter.at(ui16RayNo-1).at(ui32Temp);
}
}
cvZero( pChiThreshold );
cvmSet( pChiThreshold, 0, 0, GetPropertyFloat(“Threshhold K based on Chi-squared Distribution”) );
//if instruction for Chi-Squared test
//Mahalanobis distance is greater than determined Threshold --> Breakpoint*/
if( ( f32MinValue > *(pChiThreshold->data.fl) ) || ( ui32LineValue == 0) ){
//New Filter necessary --> generate a new one
ui32Enumerate = pKalman.size();
pKalman.push_back( cvCreateKalman(2,1,0) ); // Creation of a new filter
//Initial Post Covarinace Matrix
CvMat* P = cvCreateMat(2,2,CV_32FC1);
cvZero( P );
cvmSet( P, 0, 0, f32L );
cvmSet( P, 0, 1, 0 );
cvmSet( P, 1, 0, 0 );
cvmSet( P, 1, 1, f32L );
memcpy( pKalman.at(ui32Enumerate)->error_cov_post->data.fl, P, sizeof§);
tFloat64 f64R0 = Raypoints.at(ui16Phi).at(ui32);
//Initial Value R0 for the specific angle ui16Phi --> not the same as the initial state component at angle 0
cvSetIdentity( pKalman.at(ui32Enumerate)->state_post, cvRealScalar(f64R0) );
cvSetIdentity( pKalman.at(ui32Enumerate)->state_pre, cvRealScalar(0));
Line.push_back(vector()); // Add an empty row
tFloat64 f64 = Raypoints.at(ui16Phi).at(ui32);
Line.at(ui16Phi).push_back(f64);
LifeFilter.at(ui16RayNo).push_back(ui32Enumerate);
if(ui32 == Raypoints.at(ui16Phi).size()-1){
LifeFilter.push_back(vector()); // Add an empty row
}
}
else{ //No Breakpoint was found --> update Kalman Filter
//Prediction step of the state vector, NULL because no control matrix, xk’=Fxk-1+B*uk-1 with B=0
memcpy( (pKalman.at(ui32LineValue))->transition_matrix->data.fl, F, sizeof(F));
memcpy( (pKalman.at(ui32LineValue))->process_noise_cov->data.fl, pQk , sizeof(pQk));
//Kalman Prediction step, the (prior) state vector pointer is returned via function
cvKalmanPredict( (pKalman.at(ui32LineValue)), NULL);
CvMat* pM10 = cvCreateMat( 2, 2, CV_32FC1 );
pM10 = ( (pKalman.at(ui32LineValue))->state_pre );
CvMat *pMeasurementUpdate = cvCreateMat( 1, 1, CV_64FC1 );
cvZero( pMeasurementUpdate );
cvmSet( pMeasurementUpdate, 0, 0, Raypoints[ui16Phi][ui32] );
cvKalmanCorrect(pKalman.at(ui32LineValue), pMeasurementUpdate); //Function returns updated pKalman->state post
LifeFilter.at(ui16RayNo).push_back(ui32LineValue);
Line.at(ui32LineValue).push_back( Raypoints.at(ui16Phi).at(ui32) );
if(ui32 == Raypoints.at(ui16Phi).size()-1){
LifeFilter.push_back(vector()); // Add an empty row
}
}
}
}
}
I am using OpenCV for the realization of a Kalman Filter. Please help me, its very important.
Thanks a lot.
50jab