// get next hand position using Kalman Filter
vector<Point2f> visionUtils::getPredictedHandPosition(Point2f currentPoint, int mode)
{
vector<Point2f> predictedPoint;
// First predict, to update the internal statePre variable
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
measurement(0) = currentPoint.x;
measurement(1) = currentPoint.y;
// The update phase
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0),estimated.at<float>(1));
Point measPt(measurement(0),measurement(1));
middlePointV = measPt;
kalmanMiddlePointV = statePt;
predictedPoint.push_back(middlePointV);
predictedPoint.push_back(kalmanMiddlePointV);
return predictedPoint;
}
bool EdgeSE2OdomDifferentialCalib::write(std::ostream& os) const
{
os << measurement().vl() << " " << measurement().vr() << " " << measurement().dt();
for (int i = 0; i < information().rows(); ++i)
for (int j = i; j < information().cols(); ++j)
os << " " << information()(i, j);
return os.good();
}
void Kalman::filter(cv::Point2f center, cv::Mat &image_tracking)
{
std::ostringstream num_str;
cv::Mat_<float> measurement(2,1);
cv::Mat processNoise(4, 1, CV_32F);
//Kalman
int state_near_id = getStateIDNearest(center);
if(state_near_id <= -1)
{
return;
}
copy(kman, kman_prevs[state_near_id]);
kman.state(0) = center.x;
kman.state(1) = center.y;
//Prediction
cv::Mat prediction = kman.KF.predict();
//std::cout<<prediction <<" ";
cv::Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
//generate measurement
randn( measurement, cv::Scalar::all(0),
cv::Scalar::all(kman.KF.measurementNoiseCov.at<float>(0)));
measurement += kman.KF.measurementMatrix*kman.state;
cv::Point measPt(measurement(0), measurement(1));
//Correct
cv::Mat estimated = kman.KF.correct(measurement);
cv::Point correctPt(estimated.at<float>(0),estimated.at<float>(1));
//num_str<< i <<"-p";
AMat::drawCross(predictPt, cv::Scalar(255, 0, 0), 1, image_tracking, num_str.str());
//num_str.str("");
//num_str<< i <<"-m";
AMat::drawCross(measPt, cv::Scalar(0, 255, 0), 1, image_tracking, num_str.str());
//num_str.str("");
//num_str<< i <<"-c";
AMat::drawCross(correctPt, cv::Scalar(0, 0, 255), 1, image_tracking, num_str.str());
//num_str.str("");
/*
for (int i = 0; i < (int)predicts.size()-1; i++) {
line(image_tracking, predicts[i], predicts[i+1], Scalar(255, 0, 0), 1);
}
for (int i = 0; i < (int)measures.size()-1; i++) {
line(image_tracking, measures[i], measures[i+1], Scalar(0,255,0), 1);
}
for (int i = 0; i < (int)corrects.size()-1; i++) {
line(image_tracking, corrects[i], corrects[i+1], Scalar(0, 0, 255), 1);
}
*/
//randn( processNoise, cv::Scalar(0),
// cv::Scalar::all(sqrt(kman.KF.processNoiseCov.at<float>(0, 0))));
// kman.state = kman.KF.transitionMatrix*kman.state + processNoise;
}
bool EdgePoseLandmarkReprojectBV::write(std::ostream& os) const {
os << measurement()[0] << " "
<< measurement()[1] << " ";
//information matrix
for (int i=0; i<information().rows(); i++)
for (int j=0; j<information().cols(); j++) {
os << information()(i,j) << " ";
}
return true;
}
bool EdgeSE2::read(std::istream& is)
{
Vector3d p;
is >> p[0] >> p[1] >> p[2];
measurement().fromVector(p);
inverseMeasurement() = measurement().inverse();
for (int i = 0; i < 3; ++i)
for (int j = i; j < 3; ++j) {
is >> information()(i, j);
if (i != j)
information()(j, i) = information()(i, j);
}
return true;
}
bool EdgeProjectXYZ2UVU::read(std::istream& is)
{
for (int i=0; i<3; i++){
is >> measurement()[i];
}
inverseMeasurement() = -measurement();
for (int i=0; i<3; i++)
for (int j=i; j<3; j++) {
is >> information()(i,j);
if (i!=j)
information()(j,i)=information()(i,j);
}
return true;
}
HistoryModel::HistoryModel(QObject *parent)
: QAbstractListModel(parent)
{
// Set role names to access data from QML
QHash<int, QByteArray> roles;
roles[TimeRole] = "time";
roles[HeartRateRole] = "heartRate";
setRoleNames(roles);
// Read info file if is exists
QFile historyFile("/home/user/MyDocs/heartrate.csv");
if (historyFile.exists() && historyFile.open(QIODevice::ReadOnly)) {
QTextStream in(&historyFile);
while (!in.atEnd()) {
QString line = in.readLine();
QStringList values = line.split(" ");
if (values.length() > 1) {
QDateTime time = QDateTime::fromString(values[0], Qt::ISODate);
int value = values[1].toInt();
HeartRateMeasurement measurement(time, value);
m_data.append(measurement);
}
}
historyFile.close();
}
}
void store_run(tables::table& results)
{
if (!results.has_column("throughput"))
results.add_column("throughput");
results.set_value("throughput", measurement());
}
cv::KalmanFilter * initKF()
{
cv::KalmanFilter* kf = new cv::KalmanFilter(6,6,0);
cv::KalmanFilter KF = *kf;
cv::Mat_<float> measurement(3,1);
measurement.setTo(cv::Scalar(0));
KF.transitionMatrix = *(cv::Mat_<float>(6, 6) <<
1,0,0,1,0,0,
0,1,0,0,1,0,
0,0,1,0,0,1,
0,0,0,1,0,0,
0,0,0,0,1,0,
0,0,0,0,0,1);
cv::setIdentity(KF.measurementMatrix);
KF.statePre.at<float>(0) = 0;
KF.statePre.at<float>(1) = 0;
KF.statePre.at<float>(2) = 0;
KF.statePre.at<float>(3) = 0;
KF.statePre.at<float>(4) = 0;
KF.statePre.at<float>(5) = 0;
cv::setIdentity(KF.processNoiseCov, cv::Scalar::all(.1));
cv::setIdentity(KF.measurementNoiseCov, cv::Scalar::all(0.05));
cv::setIdentity(KF.errorCovPost, cv::Scalar::all(20));
return kf;
}
void ObjectTracker :: addNewDetection(const ObjectMatch& match)
{
m_raw_pose = match.pose()->pose3d();
cv::Vec3f r = match.pose()->pose3d().cvRodriguesRotation();
cv::Vec3f t = match.pose()->pose3d().cvTranslation();
cv::Vec3f dr = r - m_estimated_pose.cvRodriguesRotation();
cv::Vec3f dt = t - m_estimated_pose.cvTranslation();
cv::Mat1f measurement(12,1);
measurement << t[0], dt[0], t[1], dt[1], t[2], dt[2],
r[0], dr[0], r[1], dr[1], r[2], dr[2];
Mat1f new_state = m_kalman.correct(measurement);
m_estimated_pose.resetCameraTransform();
m_estimated_pose.applyTransformBeforeRodrigues(Vec3f(new_state(0,0), new_state(2,0), new_state(4,0)),
Vec3f(new_state(6,0), new_state(8,0), new_state(10,0)));
ntk_dbg_print(m_last_pose, 1);
ntk_dbg_print(m_estimated_pose , 1);
ntk_dbg_print(m_raw_pose , 1);
m_last_projected_bounding_rect = bounding_box(match.projectedBoundingRect());
m_nframes_without_detection = 0;
++m_nframes_with_detection;
m_has_updated_pose = true;
}
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());
}
void EdgeSE3OdomDifferentialCalib::computeError()
{
const VertexSE3* v1 = dynamic_cast<const VertexSE3*>(_vertices[0]);
const VertexSE3* v2 = dynamic_cast<const VertexSE3*>(_vertices[1]);
const VertexOdomParams* params = dynamic_cast<const VertexOdomParams*>(_vertices[2]);
const Isometry3d& x1 = v1->estimate();
const Isometry3d& x2 = v2->estimate();
Isometry3d odom = internal::fromVectorMQT(measurement());
Eigen::Vector3d rpy = g2o::internal::toEuler(odom.linear());
// g2o::MotionMeasurement mma(odom.translation()(0), odom.translation()(1), rpy(2), diff_time_);
// g2o::VelocityMeasurement vel = g2o::OdomConvert::convertToVelocity(mma);
// vel.setVl(params->estimate()(0) * vel.vl());
// vel.setVr(params->estimate()(1) * vel.vr());
// g2o::MotionMeasurement mmb = g2o::OdomConvert::convertToMotion(vel, params->estimate()(2));
// odom.translation()(0) = mmb.x();
// odom.translation()(1) = mmb.y();
// rpy(2) = mmb.theta();
// odom.linear() = g2o::internal::fromEuler(rpy);
// move to cpp file
// implement translation scale
// implement rotation scale, which affects odometry translation
double drift_theta = params->estimate()(1) * fabs(rpy(2));
double drift_trans = params->estimate()(2) * odom.translation().norm();
Eigen::AngleAxisd drift_rot(drift_theta + drift_trans, Eigen::Vector3d::UnitZ());
odom.translation() = params->estimate()(0) * (drift_rot * odom.translation());
rpy(2) += drift_theta + drift_trans;
odom.linear() = g2o::internal::fromEuler(rpy);
Isometry3d delta = x2.inverse() * x1 * odom;
_error = internal::toVectorMQT(delta);
}
bool Edge_V_V_GICP::write(std::ostream& os) const
{
// first point
for (int i=0; i<3; i++)
os << measurement().pos0[i] << " ";
for (int i=0; i<3; i++)
os << measurement().normal0[i] << " ";
// second point
for (int i=0; i<3; i++)
os << measurement().pos1[i] << " ";
for (int i=0; i<3; i++)
os << measurement().normal1[i] << " ";
return os.good();
}
void EdgeSE2PureCalib::computeError()
{
const VertexSE2* laserOffset = static_cast<const VertexSE2*>(_vertices[0]);
const VertexOdomDifferentialParams* odomParams = dynamic_cast<const VertexOdomDifferentialParams*>(_vertices[1]);
// get the calibrated motion given by the odometry
VelocityMeasurement calibratedVelocityMeasurment(measurement().velocityMeasurement.vl() * odomParams->estimate()(0),
measurement().velocityMeasurement.vr() * odomParams->estimate()(1),
measurement().velocityMeasurement.dt());
MotionMeasurement mm = OdomConvert::convertToMotion(calibratedVelocityMeasurment, odomParams->estimate()(2));
SE2 Ku_ij;
Ku_ij.fromVector(mm.measurement());
SE2 laserMotionInRobotFrame = laserOffset->estimate() * measurement().laserMotion * laserOffset->estimate().inverse();
SE2 delta = Ku_ij.inverse() * laserMotionInRobotFrame;
_error = delta.toVector();
}
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;
}
bool EdgeSE3PointXYZDisparity::write(std::ostream& os) const {
os << params->id() << " ";
for (int i=0; i<3; i++) os << measurement()[i] << " ";
for (int i=0; i<information().rows(); i++)
for (int j=i; j<information().cols(); j++) {
os << information()(i,j) << " ";
}
return os.good();
}
bool EdgePointXYZ::write(std::ostream& os) const
{
Vector3D p = measurement();
os << p.x() << " " << p.y() << " " << p.z();
for (int i = 0; i < 3; ++i)
for (int j = i; j < 3; ++j)
os << " " << information()(i, j);
return os.good();
}
bool EdgeSE2::write(std::ostream& os) const
{
Eigen::Vector3d p = measurement().toVector();
os << p.x() << " " << p.y() << " " << p.z();
for (int i = 0; i < 3; ++i)
for (int j = i; j < 3; ++j)
os << " " << information()(i, j);
return os.good();
}
bool G2oEdgeSim3ProjectUVQ
::read(std::istream & is)
{
for (int i=0; i<2; i++)
{
is >> measurement()[i];
}
inverseMeasurement()[0] = -measurement()[0];
inverseMeasurement()[1] = -measurement()[1];
for (int i=0; i<2; i++)
for (int j=i; j<2; j++)
{
is >> information()(i,j);
if (i!=j)
information()(j,i) = information()(i,j);
}
return true;
}
void EdgeSE3PointXYZCov::initialEstimate(const g2o::OptimizableGraph::VertexSet& from_, g2o::OptimizableGraph::Vertex* /*to_*/)
{
VertexSE3* from = static_cast<VertexSE3*>(_vertices[0]);
VertexPointXYZCov* to = static_cast<VertexPointXYZCov*>(_vertices[1]);
if (from_.count(from) > 0)
to->setEstimate(from->estimate() * measurement());
else
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
// return the error estimate as a 3-vector
void computeError()
{
// from <ViewPoint> to <Point>
const VertexPointXYZ *vp0 = static_cast<const VertexPointXYZ*>(_vertices[0]);
const VertexSE3 *vp1 = static_cast<const VertexSE3*>(_vertices[1]);
Vector3d p1 = vp1->estimate() * measurement().pos1;
_error = p1 - vp0->estimate();
}
fmat ModelCircle2D::hfun(fmat *state){
fmat measurement(state->n_rows,state->n_cols);
fmat oNoiseSample = oNoise.sample(state->n_cols);
measurement = *state + oNoiseSample;
return measurement;
}
bool EdgeSE3Prior::write(std::ostream& os) const {
os << _offsetParam->id() << " ";
for (int i=0; i<7; i++) os << measurement()[i] << " ";
for (int i=0; i<information().rows(); i++)
for (int j=i; j<information().cols(); j++) {
os << information()(i,j) << " ";
}
return os.good();
}
bool EdgeSE2XYPrior::write(std::ostream& os) const
{
Vector2d p = measurement();
os << p[0] << " " << p[1];
for (int i = 0; i < 2; ++i)
for (int j = i; j < 2; ++j)
os << " " << information()(i, j);
return os.good();
}
bool EdgePointPlane3d::write(std::ostream &os) const
{
os << measurement() << " ";
for (int i = 0; i < information().rows(); i++)
for (int j = i; j < information().cols(); j++)
os << information()(i, j) << " ";
return os.good();
}
bool EdgeSE3Expmap::write(std::ostream& os) const {
SE3Quat cam2world(measurement().inverse());
for (int i=0; i<7; i++)
os << cam2world[i] << " ";
for (int i=0; i<6; i++)
for (int j=i; j<6; j++) {
os << " " << information()(i,j);
}
return os.good();
}
KalmanResult2D KalmanFilter2D::correct( KalmanInput2D& input )
{
auto size = input.rect.size();
auto center = RectTool::center(input.rect);
this->width = size.width;
this->height = size.height;
measurement(0) = center.x;
measurement(1) = center.y;
measurement(2) = size.width;
measurement(3) = size.height;
//apply measurement
Mat estimation = filter.correct(measurement);
KalmanResult2D result;
mapMatToResult(estimation, result);
return result;
}
Point TrackShirt::kalmanTracker(Point centroid)
{
Mat prediction = KF.predict();
Point predictPt(prediction.at<float>(0),prediction.at<float>(1));
Mat_<float> measurement(2,1);
measurement(0) = centroid.x;
measurement(1) = centroid.y;
//Point measPt(measurement(0),measurement(1));
Mat estimated = KF.correct(measurement);
Point statePt(estimated.at<float>(0),estimated.at<float>(1));
//cout<<"kalman est"<<estimated<<endl;
//cout<<"kalman pt"<<statePt.x<<statePt.y<<endl;
return statePt;
}
bool EdgeProjectXYZ2UVU::write(std::ostream& os) const {
for (int i=0; i<3; i++) {
os << measurement()[i] << " ";
}
for (int i=0; i<3; i++)
for (int j=i; j<3; j++) {
os << " " << information()(i,j);
}
return os.good();
}
void EdgeSE3Offset::initialEstimate(const OptimizableGraph::VertexSet& from_, OptimizableGraph::Vertex* /*to_*/) {
VertexSE3 *from = static_cast<VertexSE3*>(_vertices[0]);
VertexSE3 *to = static_cast<VertexSE3*>(_vertices[1]);
Eigen::Isometry3d virtualMeasurement = _cacheFrom->offsetParam()->offset() * measurement() * _cacheTo->offsetParam()->offset().inverse();
if (from_.count(from) > 0) {
to->setEstimate(from->estimate() * virtualMeasurement);
} else
from->setEstimate(to->estimate() * virtualMeasurement.inverse());
}
