/**
* @brief - Initialize all the filters!
* @params- given a relX and relY for the position mean
* also a covX and covY since we may want to init
* w/ diff certainties throughout the life
* @choice I chose to have the velocities randomly initialized since there are
soooo many combos
*/
void MMKalmanFilter::initialize(float relX, float relY, float covX, float covY)
{
// clear the filters
filters.clear();
// make a random generator for initilizing different filters
boost::mt19937 rng;
rng.seed(std::time(0));
boost::uniform_real<float> posCovRange(-2.f, 2.f);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > positionGen(rng, posCovRange);
boost::uniform_real<float> randVelRange(-5.f, 5.f);
boost::variate_generator<boost::mt19937&,
boost::uniform_real<float> > velocityGen(rng, randVelRange);
//make stationary
for (int i=0; i<params.numFilters/2; i++)
{
// Needs to be stationary, have given mean, and add noise
// to the covariance matrix
KalmanFilter *stationaryFilter = new KalmanFilter(true);
ufvector4 x = boost::numeric::ublas::zero_vector<float>(4);
x(0) = relX;
x(1) = relY;
x(2) = 0.f;
x(3) = 0.f;
ufmatrix4 cov = boost::numeric::ublas::zero_matrix<float>(4);
cov(0,0) = covX + positionGen();
cov(1,1) = covY + positionGen();
// init and push it back
stationaryFilter->initialize(x, cov);
filters.push_back(stationaryFilter);
}
// make moving
for (int i=0; i<params.numFilters/2; i++)
{
// Needs to be stationary, have given mean, and add noise
// to the covariance matrix
KalmanFilter *movingFilter = new KalmanFilter(false);
ufvector4 x = boost::numeric::ublas::zero_vector<float>(4);
x(0)= relX;
x(1)= relY;
x(2) = 0.f;
x(3) = 0.f;
// Choose to assum obsv mean is perfect and just have noisy velocity
ufmatrix4 cov = boost::numeric::ublas::zero_matrix<float>(4);
cov(0,0) = covX;
cov(1,1) = covY;
cov(2,2) = 20.f;
cov(3,3) = 20.f;
movingFilter->initialize(x, cov);
filters.push_back(movingFilter);
}
}
int main(int /*argc*/, char * /*argv*/[]) {
std::srand(std::time(NULL));
/// We want a simple 2d state
typedef SimpleState<2> state_t;
/// Our process model is the simplest possible: doesn't change the mean
typedef ConstantProcess<2, state_t> process_t;
/// Create a kalman filter instance with our chosen state and process types
KalmanFilter<state_t, process_t> kf;
/// Set our process model's variance
kf.processModel.sigma = state_t::VecState::Constant(6.5);
double dt = 0.5;
double sumSquaredError = 0;
const double measurementVariance = 9; //NOISE_AMPLITUDE / 2.0;
/// for sine curve
std::vector<StatePair> data = generateSineData(dt);
/// CSV header row
std::cout << "actual x,actual y,measurement x,measurement y,filtered x,filtered y,squared error" << std::endl;
double t = 0;
for (unsigned int i = 0; i < data.size(); ++i) {
/// Predict step: Update Kalman filter by predicting ahead by dt
kf.predict(dt);
/// "take a measurement" - in this case, noisify the actual measurement
Eigen::Vector2d pos = data[i].first;
Eigen::Vector2d m = data[i].second;
AbsoluteMeasurement<state_t> meas;
meas.measurement = m;
meas.covariance = Eigen::Vector2d::Constant(measurementVariance).asDiagonal();
/// Correct step: incorporate information from measurement into KF's state
kf.correct(meas);
/// Output info for csv
double squaredError = (pos[0] - kf.state.x[0]) * (pos[0] - kf.state.x[0]);
sumSquaredError += squaredError;
std::cout << pos[0] << "," << pos[1] << ",";
std::cout << meas.measurement[0] << "," << meas.measurement[1] << ",";
std::cout << kf.state.x[0] << "," << kf.state.x[1] << ",";
std::cout << squaredError;
std::cout << std::endl;
t += dt;
}
std::cerr << "Sum squared error: " << sumSquaredError << std::endl;
return 0;
}
KalmanFilter* TargetTracker::findFilter(int grpId, int targetId)
{
KalmanFilter *filter = NULL;
filter = filters[grpId * 100000 + targetId];
if (filter == NULL) {
filter = new KalmanFilter(); // 使用默认的运动模型
filter->setGroupId(grpId);
filter->setTargetId(targetId);
filters[grpId * 100000 + targetId] = filter;
}
return filter;
}
void ocvKalmanApp::mouseDrag( MouseEvent event )
{
mMousePoints.push_back( event.getPos() );
if( mKalmanPoints.empty() ) {
mFilter = new KalmanFilter( toOcv( event.getPos() ) );
mKalmanPoints.push_back( event.getPos() );
} else {
mFilter->correct( toOcv( event.getPos() ) );
mKalmanPoints.push_back( fromOcv( mFilter->updatePrediction() ) );
console() << mKalmanPoints.back() << endl;
}
}
TEST (KalmanFilterTest, FilterCanUpdate) {
KalmanFilter *kf = new KalmanFilter(true);
kf->isUpdated();
bool yes = true;
bool no = false;
ASSERT_EQ(no, kf->isUpdated());
kf->setUpdate(true);
ASSERT_EQ(yes, kf->isUpdated());
int a =1;
int b =1;
ASSERT_EQ(a,b);
}
int main (int argc, char **argv){
int const accSigma = 1;
int const t = (argc == 2)?(lexical_cast<int>(argv[1])):(4);
double const deltaT = 1.0;
dmat A(2,2), B(2,2), G(2,1), Q(2,2);
dmat State(2,1), Cov(2,2), Ctrl(2,1);
std::vector<dmat> Result;
A(0,0) = 1;A(0,1) = deltaT;
A(1,0) = 0;A(1,1) = 1;
B(0,0) = 0;B(0,1) = 0;
B(1,0) = 0;B(1,1) = 0;
G(0,0) = deltaT * deltaT * 0.5;
G(1,0) = deltaT;
Q = accSigma * prod(G, trans(G));
State(0,0) = 0;
State(1,0) = 0;
Cov(0,0) = 1;Cov(0,1) = 0;
Cov(1,0) = 0;Cov(1,1) = 1;
Ctrl(0,0) = 0;
Ctrl(1,0) = 0;
KalmanFilter kf = KalmanFilter(&A, &B, &Q, accSigma);
for(int i = 0; i < t; ++i){
string fileName = "KFCorrect(k=" + lexical_cast<string>(i + 1) + ").txt";
ofstream ofs(fileName.c_str());
Result = kf.Correct(&State, &Cov, &Ctrl, &G);
State = Result.at(0);
Cov = Result.at(1);
for(double location = -20.0; location <= 20.0; location += 0.5){
for(double vel = -10.0; vel <= 10.0; vel += 0.5){
dmat Info(2,1);
Info(0,0) = location;
Info(1,0) = vel;
double prob = kf.calcBelief(&Info, &State, &Cov);
ofs << location << "\t" << vel << "\t" << prob << endl;
}
}
}
return 0;
}
KalmanFilter execute_time_step(KalmanFilter KF,
Mat TransitionModel,
Mat MeasurementModel,
Mat ControlModel,
Mat measurement,
Mat control)
{
KF.transitionMatrix = TransitionModel;
KF.measurementMatrix = MeasurementModel;
KF.controlMatrix = ControlModel;
KF.predict(control);
KF.correct(measurement);
return KF;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "pose_filter");
ros::NodeHandle nh;
tf::TransformListener lr;
tf::StampedTransform laser_trans, kinect_trans;
geometry_msgs::PoseStamped target_pose;
ros::Publisher pub = nh.advertise<geometry_msgs::PoseStamped>("leader_pose", 1);
target_pose.pose.position.z = 0; target_pose.pose.orientation = tf::createQuaternionMsgFromYaw(0);
target_pose.header.frame_id = "map";
Mat measurement = Mat(2, 1, CV_32FC1);
KalmanFilter kalman = KalmanFilter(4,2,0);
setup_kalman(kalman);
ros::Rate kalman_rate(KALMAN_HZ);
while(ros::ok())
{
try
{
lr.lookupTransform("map", "laser_target", ros::Time(0), laser_trans);
}
catch(tf::TransformException ex) {}
/*try
{
lr.lookupTransform("odom", "kinect_target", ros::Time(0), kinect_trans);
}
catch(tf::TransformException ex) {}
measurement.at<float>(0,0) = (laser_trans.getOrigin().x() + kinect_trans.getOrigin().x())/2;
measurement.at<float>(1,0) = (laser_trans.getOrigin().y() + kinect_trans.getOrigin().y())/2;*/
measurement.at<float>(0,0) = laser_trans.getOrigin().x();
measurement.at<float>(1,0) = laser_trans.getOrigin().y();
Mat predict = kalman.predict(); kalman.correct(measurement);
target_pose.pose.position.x = kalman.statePost.at<float>(0,0);
target_pose.pose.position.y = kalman.statePost.at<float>(1,0);
target_pose.header.stamp = laser_trans.stamp_;
pub.publish(target_pose);
kalman_rate.sleep();
}
return 0;
}
void initKalman(float x, float y)
{
// Instantate Kalman Filter with
// 4 dynamic parameters and 2 measurement parameters,
// where my measurement is: 2D location of object,
// and dynamic is: 2D location and 2D velocity.
KF.init(4, 2, 0);
measurement = Mat_<float>::zeros(2,1);
measurement.at<float>(0, 0) = x;
measurement.at<float>(0, 0) = y;
KF.statePre.setTo(0);
KF.statePre.at<float>(0, 0) = x;
KF.statePre.at<float>(1, 0) = y;
KF.statePost.setTo(0);
KF.statePost.at<float>(0, 0) = x;
KF.statePost.at<float>(1, 0) = y;
KF.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,1,0, 0,1,0,1, 0,0,1,0, 0,0,0,1);
setIdentity(KF.measurementMatrix);
setIdentity(KF.processNoiseCov, Scalar::all(.005)); //adjust this for faster convergence - but higher noise
setIdentity(KF.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(KF.errorCovPost, Scalar::all(.1));
}
// startTracking()
//
void startTracking(IplImage * pImg, CvRect pHandRect,KalmanFilter &kfilter)
{
float maxVal = 0.f;
// Make sure internal data structures have been allocated
if( !pHist ) createTracker(pImg);
// Create a new hue image
updateHueImage(pImg);
if(!((pHandRect.x<0)||(pHandRect.y<0)||((pHandRect.x+pHandRect.width)>pImg->width)||((pHandRect.y+pHandRect.height)>pImg->height))) {
// Create a histogram representation for the hand
cvSetImageROI( pHueImg, pHandRect );
cvSetImageROI( pMask, pHandRect );
cvCalcHist( &pHueImg, pHist, 0, pMask );
cvGetMinMaxHistValue( pHist, 0, &maxVal, 0, 0 );
cvConvertScale( pHist->bins, pHist->bins, maxVal? 255.0/maxVal : 0, 0 );
cvResetImageROI( pHueImg );
cvResetImageROI( pMask );
}
// Store the previous hand location
prevHandRect =pHandRect;
prevHandRect2 =pHandRect;
//Pass the hand location to kalman initializer
kfilter.predictionBegin(prevHandRect);
}
int Kalman_adjust(Point A,int biggest,KalmanFilter &F)
{
Mat_<float> measurement(2,1);
if(!notstarted[biggest])
{
notstarted[biggest]=true;
measurement.setTo(Scalar(0));
F.statePre.at<float>(0) = A.x;
F.statePre.at<float>(1) = A.y;
F.statePre.at<float>(2) = 0;
F.statePre.at<float>(3) = 0;
F.transitionMatrix = *(Mat_<float>(4, 4) << 1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1);
setIdentity(F.measurementMatrix);
setIdentity(F.processNoiseCov, Scalar::all(1e-4));
setIdentity(F.measurementNoiseCov, Scalar::all(1e-1));
setIdentity(F.errorCovPost, Scalar::all(.1));
}
else
{
measurement(0) = A.x;
measurement(1) = A.y;
Mat estimated = F.correct(measurement);
Point statePt(estimated.at<float>(0),estimated.at<float>(1));
}
}
CvRect combi_track(IplImage * pImg,KalmanFilter &kfilter)
{
CvRect predrect=kfilter.predictionReport(prevHandRect);
//if((predrect.x<0)||(predrect.y<0)||((predrect.x+predrect.width)>pImg->width)||((predrect.y+predrect.height)>pImg->height))
// return NULL;
CvConnectedComp components;
// Create a new hue image
updateHueImage(pImg);
// Create a probability image based on the hand histogram
cvCalcBackProject( &pHueImg, pProbImg, pHist );
cvAnd( pProbImg, pMask, pProbImg, 0 );
//cvSetImageROI(pProbImg,predrect);
// Use CamShift to find the center of the new hand probability
if(!((predrect.x<0)||(predrect.y<0)||((predrect.x+predrect.width)>pImg->width)||((predrect.y+predrect.height)>pImg->height))) {
cvCamShift( pProbImg, predrect, cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),&components, handBox );
// Update hand location and angle
prevHandRect = components.rect;
}
else
//cvCamShift( pProbImg, prevHandRect, cvTermCriteria( CV_TERMCRIT_EPS | CV_TERMCRIT_ITER, 10, 1 ),&components, handBox );
prevHandRect.x=-1;
//if(!pImg->origin)
// handBox->angle = -handBox->angle;
//cvResetImageROI(pProbImg);
return prevHandRect;
}
//-----------------------------------------------------------------------------
//
void setup()
{
Serial.begin(9600);
Wire.begin();
i2cData[0] = 7; // Set the sample rate to 1000Hz - 8kHz/(7+1) = 1000Hz
i2cData[1] = 0x00; // Disable FSYNC and set 260 Hz Acc filtering, 256 Hz Gyro filtering, 8 KHz sampling
i2cData[2] = 0x00; // Set Gyro Full Scale Range to ±250deg/s
i2cData[3] = 0x00; // Set Accelerometer Full Scale Range to ±2g
while(i2cWrite(0x19,i2cData,4,false)); // Write to all four registers at once
while(i2cWrite(0x6B,0x01,true)); // PLL with X axis gyroscope reference and disable sleep mode
while(i2cRead(0x75,i2cData,1));
if(i2cData[0] != 0x68) // Read "WHO_AM_I" register
{
Serial.print(F("Error reading sensor"));
while(1);
}
delay(100); // Wait for sensor to stabilize
/* Set kalman and gyro starting angle */
while(i2cRead(0x3B,i2cData,6));
accX = ((i2cData[0] << 8) | i2cData[1]);
accY = ((i2cData[2] << 8) | i2cData[3]);
accZ = ((i2cData[4] << 8) | i2cData[5]);
// atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
// We then convert it to 0 to 2π and then from radians to degrees
accYangle = (atan2(accX,accZ)+PI)*RAD_TO_DEG;
accXangle = (atan2(accY,accZ)+PI)*RAD_TO_DEG;
kalmanX.setAngle(accXangle); // Set starting angle
kalmanY.setAngle(accYangle);
gyroXangle = accXangle;
gyroYangle = accYangle;
compAngleX = accXangle;
compAngleY = accYangle;
TCCR2B = TCCR2B & 0b11111000 | 0x05;
m1.Initialize();
m2.Initialize();
m1.Activate();
m2.Activate();
timer = micros();
}
cv::Point getKalmanCentroid(int x, int y)
{
cv::Point kalman_centroid;
mouse_info.x = x;
mouse_info.y = y;
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
measurement(0) = mouse_info.x;
measurement(1) = mouse_info.y;
Point measPt(measurement(0),measurement(1));
mousev.push_back(measPt);
// generate measurement
//measurement += KF.measurementMatrix*state;
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0),estimated.at<float>(1));
kalmanv.push_back(statePt);
//drawCross( statePt, Scalar(0,255,0), 5 ); // Kalman
//drawCross( measPt, Scalar(0,0,255), 5 ); // Original
// for (int i = 0; i < mousev.size()-1; i++) {
// line(kalman_img, mousev[i], mousev[i+1], Scalar(255,255,0), 1);
// }
// for (int i = 0; i < kalmanv.size()-1; i++) {
// line(kalman_img, kalmanv[i], kalmanv[i+1], Scalar(0,255,0), 1);
// }
// if (contador > 30)
// {
// initializeKalman();
// }
// else
// {
// //ROS_INFO(">>> MEDIDA(%d,%d) KALMAN(%d,%d)", measPt.x, measPt.y, statePt.x, statePt.y);
// }
//imshow("mi img", img);
//imshow("Kalman", kalman_img);
kalman_centroid.x = statePt.x;
kalman_centroid.y = statePt.y;
return kalman_centroid;
}
Point kalmanCorrect(float x, float y)
{
measurement(0) = x;
measurement(1) = y;
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0),estimated.at<float>(1));
return statePt;
}
vector<Point> Widget::findKalmanCenters(vector<Point> dataInput){
vector<Point> kalmanCenters;
if(dataInput.empty()){
errorMsg("Nao ha objetos a serem detectados!");
return kalmanCenters;
}
kalmanCenters.resize(dataInput.size());
KalmanFilter *KF;
for(unsigned int i = 0; i < targets.size(); i++){
KF = targets[i]->KF;
//apenas conversao de estruturas usadas
Mat_<float> measurement(2,1);
measurement(0) = dataInput[i].x;
measurement(1) = dataInput[i].y;
Mat estimated;
Mat prediction = KF->predict();
if(detectionSucess[i] == true)
{
//caso a detecção tenha sido um sucesso jogamos a nova medida no filtro
//ao chamar correct já estamos adicionando a nova medida ao filtro
estimated = KF->correct(measurement);
}
else{
/*caso a medição tenha falhada realimentamos o filtro com a própria
predição anterior*/
Mat_<float> predictedMeasurement(2,1);
predictedMeasurement(0) = prediction.at<float>(0);
predictedMeasurement(1) = prediction.at<float>(1);
estimated = KF->correct(predictedMeasurement);
KF->errorCovPre.copyTo(KF->errorCovPost);
//copiar a covPre para covPos [dica de um usuário de algum fórum]
}
Point statePt(estimated.at<float>(0),estimated.at<float>(1));
kalmanCenters[i] = statePt;
/*existe o centro medido pela previsão e o centro que o filtro de kalman
acredita ser o real. O centro de kalman é uma ponderação das medidas e do modelo
com conhecimento prévio de um erro aleatório*/
}
return kalmanCenters;
}
int Kalman_Perdict(KalmanFilter &F)
{
if(notstarted[0] and notstarted[1] and notstarted[2])
{
Mat prediction = F.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
MyFilledCircle(skin2,predictPt,1);
}
return 0;
}
void fc_manual_alt0_change(float val)
{
kalmanFilter.reset(val);
if (fc.flight.state == FLIGHT_WAIT || fc.flight.state == FLIGHT_LAND)
{
fc.flight.autostart_altitude = val;
fc.altitude1 = val;
}
}
JNIEXPORT jlong JNICALL Java_org_opencv_video_KalmanFilter_predict_11
(JNIEnv* env, jclass , jlong self)
{
try {
LOGD("video::predict_11()");
KalmanFilter* me = (KalmanFilter*) self; //TODO: check for NULL
Mat _retval_ = me->predict( );
return (jlong) new Mat(_retval_);
} catch(cv::Exception e) {
LOGD("video::predict_11() catched cv::Exception: %s", e.what());
jclass je = env->FindClass("org/opencv/core/CvException");
if(!je) je = env->FindClass("java/lang/Exception");
env->ThrowNew(je, e.what());
return 0;
} catch (...) {
LOGD("video::predict_11() catched unknown exception (...)");
jclass je = env->FindClass("java/lang/Exception");
env->ThrowNew(je, "Unknown exception in JNI code {video::predict_11()}");
return 0;
}
}
void TargetTracker::tracking(std::vector<TargetState> states)
{
std::vector<TargetState>::const_iterator iter;
TargetState tState;
KalmanFilter *filter;
SingleTarget *target;
qDebug() << "测量值：";
for (iter = states.begin(); iter != states.end(); ++iter)
{
tState = *iter;
tState.state.print();
filter = findFilter(tState.groupId, tState.targetId);
target = findTarget(tState.groupId, tState.targetId);
if (target == NULL) continue;
if (filter == NULL) continue;
if (target->getStateCount() == 0) {
// 第一次获取目标的状态，直接保存，不做任何计算。
target->addState(tState.state);
filter->setState(tState.state.getData());
continue;
}
//计算一步预测值
Matrix matrix = filter->estimate();
// 根据测量值更新预测数据
filter->updateByMeasure(tState.state.getData());
State s;
s.setData(matrix);
s.setTime(tState.time);
target->addState(s);
}
//项目验收后去掉
initMessage();
printTargetGroups();
}
vector<vector<Point> > Widget::predictFuture(int futureSize){
vector<vector<Point> > future;
future.resize(targets.size());
for(unsigned int i = 0; i < targets.size(); i++)
{
KalmanFilter *KF;
KF = targets[i]->KF;
KalmanFilter DelfusOracle = myMath::copyKF(*KF);
/*para ver o futuro copiamos o estado do filtro atual e
o realimentamos com suas próprias previsões um número fixo de vezes*/
for(int j = 0; j < futureSize; j++)
//CALCULA PONTOS DO FUTURO
{
Mat prediction = DelfusOracle.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
future[i].push_back(predictPt);
Mat_<float> predictedMeasurement(2,1);
predictedMeasurement(0) = prediction.at<float>(0);
predictedMeasurement(1) = prediction.at<float>(1);
DelfusOracle.correct(predictedMeasurement);
DelfusOracle.errorCovPre.copyTo(DelfusOracle.errorCovPost);
//copiamos covPre para covPost seguindo a dica de um fórum
//o Vidal não gosta dessa dica, diz ele que isso engana o filtro
}
}//end for each color
return future;
}
/*
* box_update
* Updates the box that we are going to search for the template
* match in to be centered over the center of the current match
* location.
*
* Inputs:
* kal -> reference of a kalman filter object that is used to track and predict the match location
* bb -> rectangle that is the size of the training image used to find the best point and is in the location of the match
*
* Outputs:
* measurement -> the point to store the measured point
* ctr_point -> the center point of the best match.
* kal_point -> the center point adjusted by the kalman filter
*/
void box_update(KalmanFilter &kal, Rect &bb, Mat_<float> &measurement, Point2f &ctr_point, Point2f &kal_point){
Point measp = Point(bb.tl().x + (bb.width / 2), bb.tl().y + (bb.height / 2));
//save measured matrix
measurement(0) = measp.x;
measurement(1) = measp.y;
ctr_point = measp; //save measured center point
Mat estimated = kal.correct(measurement); //correct the predicted center with the measurement point
Point statePt(estimated.at<float>(0),estimated.at<float>(1)); //the corrected center
kal_point = statePt; //save the corrected point
//update selection so next time we have the mask in the right spot
selection.x = statePt.x - (bb.width/2);
selection.y = statePt.y - (bb.height/2);
selection.width = bb.width;
selection.height = bb.height;
}
int IMUTester::setupTester(void)
{
#ifdef SERIAL_USB
Serial.begin(115200);
sensorMode = ALL;
#endif
#ifdef SERIAL_BLUETOOTH
Serial1.begin(115200, SERIAL_8N1); // baud rate = 115200, 8 data bits, no parity, 1 stop bit
Serial1.println("R,1"); // restart it
#endif
kalman.initialize( 6.9158f, 9.9410f, 21.515f,
-16.027f, 0.9157f, 0.6185f,
// -12504.f, -13316.f, -24942.f );
0,0,0);
i2c.initialize();
serialPrint("IMUTester::setupTester called");
//acc.setup();
accel.begin();
gyro.initialize();
//dataRate_t maxDataRate = ADXL345_DATARATE_3200_HZ;
//accel.setDataRate(maxDataRate);
//gyro.setRate(2);
delay(1000);
//bar.setup();
comp.setup();
//gpssetup {
Serial2.begin(57600);
delay(1000);
//}
// Serial1.begin(115200, SERIAL_8N1); // baud rate = 115200, 8 data bits, no parity, 1 stop bit
// Serial1.println("R,1");
cycles = 1;
sumCollectionTime = 5;
//serialPrint("a.x,a.y,a.z,g.x,g.y,g.z,c.x,c.y,G.x,G.y,G.z,G.a");
finish = millis() + 100;
return 0;
}
TEST (KalmanFilterTest, MMWorks) {
MMKalmanFilter *kf = new MMKalmanFilter();
KalmanFilter *poor = new KalmanFilter();
kf->initialize(50.f, 0.f, 5.f, 5.f);
messages::Motion inpMotion;
inpMotion.mutable_odometry()->CopyFrom(poor->genOdometry(0.f,0.f,0.f));
kf->update(poor->genVisBall(55.5f, 0.f),
inpMotion);
kf->update(poor->genVisBall(62.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(64.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(71.f, 0.f),
inpMotion);
kf->printEst();
/*
kf->update(poor->genVisBall(75.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(74.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(76.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(75.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(75.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(75.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(75.f, 0.f),
inpMotion);
kf->update(poor->genVisBall(75.f, 0.f),
inpMotion);
*/
}
TEST (KalmanFilterTest, FilterCanPredict) {
KalmanFilter *kf = new KalmanFilter(false);
kf->initialize();
float initX = 0.f;
float initY = 0.f;
float initXVel = 1.f;
float initYVel = 0.5f;
float finalX = 4.f;
float finalY = 2.f;
ASSERT_EQ(initX, kf->getRelXPosEst());
ASSERT_EQ(initY, kf->getRelYPosEst());
messages::RobotLocation odometry;
odometry.set_x(0.f);
odometry.set_y(0.f);
odometry.set_h(0.f);
kf->predict(odometry, 4.f);
ASSERT_EQ(finalX, kf->getRelXPosEst());
ASSERT_EQ(finalY, kf->getRelYPosEst());
}
Point kalmanPredict()
{
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
return predictPt;
}
TEST(KalmanFilterTest, validationEffect)
{
KalmanFilter kf;
MatrixXd A(1,1); A << 1;
kf.setStateTransitionModel(A);
MatrixXd H(1,1); H << 1;
kf.setObservationModel(H);
MatrixXd Q(1,1); Q << 0.1;
kf.setProcessNoiseCovariance(Q);
MatrixXd R(1,1); R << 10;
kf.setMeasurementNoiseCovariance(R);
// validate has an effect?
InputValue measurement(1);
Input in(measurement);
Output out;
EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error); // "not yet validated"
EXPECT_NO_THROW(kf.validate());
// changing a parameter -> KF must be re-validated
kf.setProcessNoiseCovariance(Q);
EXPECT_THROW(out = kf.estimate(in), IEstimator::estimator_error); // "not yet validated"
EXPECT_NO_THROW(kf.validate());
// KF should now be released and return an estimate (should not be
// the default)
EXPECT_NO_THROW(out = kf.estimate(in));
EXPECT_GT(out.size(), 0);
EXPECT_EQ(kf.getState().size(), out.size());
}
TEST (KalmanFilterTest, KalmanFilterCanInitialize) {
KalmanFilter *kf = new KalmanFilter();
kf->initialize();
ASSERT_EQ(kf->getRelXPosEst(), 0.f);
ASSERT_EQ(kf->getRelYPosEst(), 0.f);
ASSERT_EQ(kf->getRelXVelEst(), 1.f);
ASSERT_EQ(kf->getRelYVelEst(), 0.5f);
ufvector4 initX = boost::numeric::ublas::zero_vector<float> (4);
initX(0) = 10.f;
initX(1) = 11.f;
initX(2) = 12.f;
initX(3) = 13.f;
ufmatrix4 initCov = boost::numeric::ublas::identity_matrix<float> (4);
initCov(0,0) = 1.f;
initCov(1,1) = 2.f;
initCov(2,2) = 3.f;
initCov(3,3) = 4.f;
kf->initialize(initX, initCov);
ASSERT_EQ(kf->getRelXPosEst(), 10.f);
ASSERT_EQ(kf->getRelYPosEst(), 11.f);
ASSERT_EQ(kf->getRelXVelEst(), 12.f);
ASSERT_EQ(kf->getRelYVelEst(), 13.f);
ASSERT_EQ(kf->getCovXPosEst(), 1.f);
ASSERT_EQ(kf->getCovYPosEst(), 2.f);
ASSERT_EQ(kf->getCovXVelEst(), 3.f);
ASSERT_EQ(kf->getCovYVelEst(), 4.f);
}
TEST (KalmanFilterTest, Works) {
KalmanFilter *kf = new KalmanFilter(true);
ufvector4 initX = boost::numeric::ublas::zero_vector<float> (4);
initX(0) = 50.f;
initX(1) = 0.f;
initX(2) = 0.f;
initX(3) = 0.f;
ufmatrix4 initCov = boost::numeric::ublas::identity_matrix<float> (4);
initCov(0,0) = 5.f;
initCov(1,1) = 5.f;
initCov(2,2) = 0.f;
initCov(3,3) = 0.f;
kf->initialize(initX, initCov);
kf->predict(kf->genOdometry(-5.f, 0.f, 0.f),10.f);
kf->updateWithObservation(kf->genVisBall(58.f, 0.f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(55.f, .05f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(55.f, .05f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(55.f, .05f));
kf->predict(kf->genOdometry(0.f, 0.f, 0.f),.1f);
kf->updateWithObservation(kf->genVisBall(83.f, .05f));
}
void CHumanTracker::detectAndTrackFace()
{
static ros::Time probe;
// Do ROI
debugFrame = rawFrame.clone();
Mat img = this->rawFrame(searchROI);
faces.clear();
ostringstream txtstr;
const static Scalar colors[] = { CV_RGB(0,0,255),
CV_RGB(0,128,255),
CV_RGB(0,255,255),
CV_RGB(0,255,0),
CV_RGB(255,128,0),
CV_RGB(255,255,0),
CV_RGB(255,0,0),
CV_RGB(255,0,255)} ;
Mat gray;
Mat frame( cvRound(img.rows), cvRound(img.cols), CV_8UC1 );
cvtColor( img, gray, CV_BGR2GRAY );
resize( gray, frame, frame.size(), 0, 0, INTER_LINEAR );
//equalizeHist( frame, frame );
// This if for internal usage
const ros::Time _n = ros::Time::now();
double dt = (_n - probe).toSec();
probe = _n;
CvMat _image = frame;
if (!storage.empty())
{
cvClearMemStorage(storage);
}
CvSeq* _objects = cvHaarDetectObjects(&_image, cascade, storage,
1.2, initialScoreMin, CV_HAAR_DO_CANNY_PRUNING|CV_HAAR_SCALE_IMAGE, minFaceSize, maxFaceSize);
vector<CvAvgComp> vecAvgComp;
Seq<CvAvgComp>(_objects).copyTo(vecAvgComp);
// End of using C API
isFaceInCurrentFrame = (vecAvgComp.size() > 0);
// This is a hack
bool isProfileFace = false;
if ((profileHackEnabled) && (!isFaceInCurrentFrame) && ((trackingState == STATE_REJECT) || (trackingState == STATE_REJECT)))
{
ROS_DEBUG("Using Profile Face hack ...");
if (!storageProfile.empty()) {
cvClearMemStorage(storageProfile);
}
CvSeq* _objectsProfile = cvHaarDetectObjects(&_image, cascadeProfile, storageProfile,
1.2, initialScoreMin, CV_HAAR_DO_CANNY_PRUNING|CV_HAAR_SCALE_IMAGE, minFaceSize, maxFaceSize);
vecAvgComp.clear();
Seq<CvAvgComp>(_objectsProfile).copyTo(vecAvgComp);
isFaceInCurrentFrame = (vecAvgComp.size() > 0);
if (isFaceInCurrentFrame)
{
ROS_DEBUG("The hack seems to work!");
}
isProfileFace = true;
}
if (trackingState == STATE_LOST)
{
if (isFaceInCurrentFrame)
{
stateCounter++;
trackingState = STATE_DETECT;
}
}
if (trackingState == STATE_DETECT)
{
if (isFaceInCurrentFrame)
{
stateCounter++;
}
else
{
stateCounter = 0;
trackingState = STATE_LOST;
}
if (stateCounter > minDetectFrames)
{
stateCounter = 0;
trackingState = STATE_TRACK;
}
}
if (trackingState == STATE_TRACK)
{
if (!isFaceInCurrentFrame)
{
trackingState = STATE_REJECT;
}
}
if (trackingState == STATE_REJECT)
{
float covNorm = sqrt(
pow(KFTracker.errorCovPost.at<float>(0,0), 2) +
pow(KFTracker.errorCovPost.at<float>(1,1), 2)
);
if (!isFaceInCurrentFrame)
{
stateCounter++;
}
else
{
stateCounter = 0;
trackingState = STATE_TRACK;
}
if ((stateCounter > minRejectFrames) && (covNorm > maxRejectCov))
{
trackingState = STATE_LOST;
stateCounter = 0;
resetKalmanFilter();
reset();
}
}
if ((trackingState == STATE_TRACK) || (trackingState == STATE_REJECT))
{
bool updateFaceHist = false;
// This is important:
KFTracker.transitionMatrix.at<float>(0,2) = dt;
KFTracker.transitionMatrix.at<float>(1,3) = dt;
Mat pred = KFTracker.predict();
if (isFaceInCurrentFrame)
{
//std::cout << vecAvgComp.size() << " detections in image " << std::endl;
float minCovNorm = 1e24;
int i = 0;
for( vector<CvAvgComp>::const_iterator rr = vecAvgComp.begin(); rr != vecAvgComp.end(); rr++, i++ )
{
copyKalman(KFTracker, MLSearch);
CvRect r = rr->rect;
r.x += searchROI.x;
r.y += searchROI.y;
double nr = rr->neighbors;
Point center;
Scalar color = colors[i%8];
float normFaceScore = 1.0 - (nr / 40.0);
if (normFaceScore > 1.0) normFaceScore = 1.0;
if (normFaceScore < 0.0) normFaceScore = 0.0;
setIdentity(MLSearch.measurementNoiseCov, Scalar_<float>::all(normFaceScore));
center.x = cvRound(r.x + r.width*0.5);
center.y = cvRound(r.y + r.height*0.5);
measurement.at<float>(0) = r.x;
measurement.at<float>(1) = r.y;
measurement.at<float>(2) = r.width;
measurement.at<float>(3) = r.height;
MLSearch.correct(measurement);
float covNorm = sqrt(
pow(MLSearch.errorCovPost.at<float>(0,0), 2) +
pow(MLSearch.errorCovPost.at<float>(1,1), 2)
);
if (covNorm < minCovNorm)
{
minCovNorm = covNorm;
MLFace = *rr;
}
// if ((debugLevel & 0x02) == 0x02)
// {
rectangle(debugFrame, center - Point(r.width*0.5, r.height*0.5), center + Point(r.width*0.5, r.height * 0.5), color);
txtstr.str("");
txtstr << " Sc:" << rr->neighbors << " S:" << r.width << "x" << r.height;
putText(debugFrame, txtstr.str(), center, FONT_HERSHEY_PLAIN, 1, color);
// }
}
// TODO: I'll fix this shit
Rect r(MLFace.rect);
r.x += searchROI.x;
r.y += searchROI.y;
faces.push_back(r);
double nr = MLFace.neighbors;
faceScore = nr;
if (isProfileFace) faceScore = 0.0;
float normFaceScore = 1.0 - (nr / 40.0);
if (normFaceScore > 1.0) normFaceScore = 1.0;
if (normFaceScore < 0.0) normFaceScore = 0.0;
setIdentity(KFTracker.measurementNoiseCov, Scalar_<float>::all(normFaceScore));
measurement.at<float>(0) = r.x;
measurement.at<float>(1) = r.y;
measurement.at<float>(2) = r.width;
measurement.at<float>(3) = r.height;
KFTracker.correct(measurement);
// We see a face
updateFaceHist = true;
}
else
{
KFTracker.statePost = KFTracker.statePre;
KFTracker.errorCovPost = KFTracker.errorCovPre;
}
// TODO: MOVE THIS
for (unsigned int k = 0; k < faces.size(); k++) {
rectangle(debugFrame, faces.at(k), CV_RGB(128, 128, 128));
}
beleif.x = max<int>(KFTracker.statePost.at<float>(0), 0);
beleif.y = max<int>(KFTracker.statePost.at<float>(1), 0);
beleif.width = min<int>(KFTracker.statePost.at<float>(4), iWidth - beleif.x);
beleif.height = min<int>(KFTracker.statePost.at<float>(5), iHeight - beleif.y);
Point belCenter;
belCenter.x = beleif.x + (beleif.width * 0.5);
belCenter.y = beleif.y + (beleif.height * 0.5);
if ((debugLevel & 0x02) == 0x02)
{
txtstr.str("");
// txtstr << "P:" << std::setprecision(3) << faceUncPos << " S:" << beleif.width << "x" << beleif.height;
// putText(debugFrame, txtstr.str(), belCenter + Point(0, 50), FONT_HERSHEY_PLAIN, 2, CV_RGB(255,0,0));
// circle(debugFrame, belCenter, belRad, CV_RGB(255,0,0));
// circle(debugFrame, belCenter, (belRad - faceUncPos < 0) ? 0 : (belRad - faceUncPos), CV_RGB(255,255,0));
// circle(debugFrame, belCenter, belRad + faceUncPos, CV_RGB(255,0,255));
}
//searchROI.x = max<int>(belCenter.x - KFTracker.statePost.at<float>(4) * 2, 0);
//searchROI.y = max<int>(belCenter.y - KFTracker.statePost.at<float>(5) * 2, 0);
//int x2 = min<int>(belCenter.x + KFTracker.statePost.at<float>(4) * 2, iWidth);
//int y2 = min<int>(belCenter.y + KFTracker.statePost.at<float>(4) * 2, iHeight);
//searchROI.width = x2 - searchROI.x;
//searchROI.height = y2 - searchROI.y;
if ((updateFaceHist) && (skinEnabled))
{
//updateFaceHist is true when we see a real face (not all the times)
Rect samplingWindow;
// samplingWindow.x = beleif.x + (0.25 * beleif.width);
// samplingWindow.y = beleif.y + (0.1 * beleif.height);
// samplingWindow.width = beleif.width * 0.5;
// samplingWindow.height = beleif.height * 0.9;
samplingWindow.x = measurement.at<float>(0) + (0.25 * measurement.at<float>(2));
samplingWindow.y = measurement.at<float>(1) + (0.10 * measurement.at<float>(3));
samplingWindow.width = measurement.at<float>(2) * 0.5;
samplingWindow.height = measurement.at<float>(3) * 0.9;
if ((debugLevel & 0x04) == 0x04)
{
rectangle(debugFrame, samplingWindow, CV_RGB(255,0,0));
}
Mat _face = rawFrame(samplingWindow);
generateRegionHistogram(_face, faceHist);
}
}
// if ((debugLevel & 0x02) == 0x02)
// {
// rectangle(debugFrame, searchROI, CV_RGB(0,0,0));
// txtstr.str("");
// txtstr << strStates[trackingState] << "(" << std::setprecision(3) << (dt * 1e3) << "ms )";
// putText(debugFrame, txtstr.str() , Point(30,300), FONT_HERSHEY_PLAIN, 2, CV_RGB(255,255,255));
// }
// dt = ((double) getTickCount() - t) / ((double) getTickFrequency()); // In Seconds
}