コード例 #1
0
ファイル: object_tracker.cpp プロジェクト: Makeroni/nestk
  ObjectTracker::ObjectTracker(const ObjectMatch& match)
    : m_nframes_without_detection(0),
    m_nframes_with_detection(0),
    m_has_updated_pose(false)
  {
    m_last_pose = match.pose()->pose3d();
    m_raw_pose = match.pose()->pose3d();
    m_estimated_pose = m_last_pose;
    m_model = &(match.model());
    m_last_projected_bounding_rect = bounding_box(match.pose()->projectedBoundingRect());

    /*
     * 12 variables
     * tx, Dtx, ty, Dy, tz, Dtz, rx, Drx, ry, Dry, rz, Drz
     * with D the derivative.
     * This tracking assumes a constant speed model.
     * Rotation are given as axis / angle as magnitude
     * representation.
     */
    m_kalman = KalmanFilter(12, 12, 0);
    Mat1f state = Mat(12, 1, CV_32FC1);

    Mat1f process_noise (12, 1);
    Mat1f measurement (12, 1);
    Mat1f measurement_noise (12, 1);

    measurement = 0.f;

    /*
     * If there were only two variables, this generates a matrix
     * with the following form:
     * 1 1 0 0
     * 0 1 0 0
     * 0 0 1 1
     * 0 0 1 0
     */
    m_kalman.transitionMatrix = Mat1f(12, 12);
    setIdentity(m_kalman.transitionMatrix);
    for (int i = 0; i < 12; i += 2)
    {
      m_kalman.transitionMatrix.at<float>(i,i+1) = 1;
    }

    setIdentity(m_kalman.measurementMatrix, 1);
    setIdentity(m_kalman.processNoiseCov, 1e-5);
    setIdentity(m_kalman.measurementNoiseCov, 1e-1);
    setIdentity(m_kalman.errorCovPost, 1);

    cv::Vec3f r = m_last_pose.cvRodriguesRotation();
    cv::Vec3f t = m_last_pose.cvTranslation();
    // derivatives are null initially.
    ((Mat1f)m_kalman.statePost) << t[0], 0, t[1], 0, t[2], 0, r[0], 0, r[1], 0, r[2], 0;
  }
コード例 #2
0
ファイル: ukf_test.cpp プロジェクト: contradict/SampleReturn
void
runTest(void)
{
    double t=0;
    double dt=0.01;
    int N=400;
    std::vector<double> positions;
    std::vector<double> velocities;
    positions.resize(N);
    velocities.resize(N);
    std::generate_n(positions.begin(),
            N,
            [&t, &dt](void)
            {
               double x=sin(t*2*M_PI);
               t += dt;
               return x;
            });
    t = 0.0;
    std::generate_n(velocities.begin(),
            N,
            [&t, &dt](void)
            {
               double x=2*M_PI*cos(t*2*M_PI);
               t += dt;
               return x;
            });
    std::vector<double> noisy_positions;
    noisy_positions.resize(N);
    std::mt19937 generator;
    std::normal_distribution<double> normal(0.0, 0.1);
    std::transform(positions.begin(),
            positions.end(),
            noisy_positions.begin(),
            [&normal, &generator](double x)
            {
               return x+normal(generator);
            });

    TestUKF ukf(1e-3, 2.0, 0.0);
    // acceleration noise
    double sigma = (2*M_PI)*(2*M_PI)*10;
    TestState st;
    st.v=0.0;
    ukf.reset(st);

    Eigen::MatrixXd measurement_noise(1,1);
    measurement_noise(0,0) = 0.02;
    std::vector<Eigen::MatrixXd> mcovs;
    mcovs.push_back(measurement_noise);

    t = 0.0;
    std::cout << t << ", " <<
                 positions[0] << ", " <<
                 velocities[0] << ", " <<
                 ukf.state().x << ", " <<
                 sqrt(ukf.state().Covariance(0,0)) << ", " <<
                 ukf.state().v << ", " <<
                 sqrt(ukf.state().Covariance(1,1)) << ", " <<
                 0.0 << std::endl;
    int correct_every=1;
    for(int i=0;i<N;i++)
    {
        ukf.predict(dt, process_noise(dt, sigma));
        t += dt;
        PositionMeasurement m;
        m.x = noisy_positions[i];
        if(i%correct_every==0)
            ukf.correct(m, mcovs);
        std::cout << t << ", " <<
                     positions[i] << ", " <<
                     velocities[i] << ", " <<
                     ukf.state().x << ", " <<
                     sqrt(ukf.state().Covariance(0,0)) << ", " <<
                     ukf.state().v << ", " <<
                     sqrt(ukf.state().Covariance(1,1)) << ", " <<
                     m.x << ", " <<
                     sqrt(measurement_noise(0,0)) <<
                     std::endl;
    }
}