ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Publishing Velocity of Kalman Filtered Clusters

asked 2018-08-12 13:31:09 -0600

th6262 gravatar image

Hello everyone, I've segmented an incoming Point Cloud, applied an euclidian cluster extraction and then associated the data between frames combined with a Kalman filter.

Right now I'm publishing text markers above every object, containing the object id, x and y position. I wanna publish the x and y velocity ( from the Kalman filter) aswell , but I'm struggling to find out how. In Line 780 - 785 I've tried to so so with a geometry:msgs::Twist Publisher, but it doesn't publish anything.

I would appreciate any suggestion and thank you in advance!

using namespace std;
using namespace cv;

std::string stringify(float value)
     std::ostringstream oss;
     oss << value;
     return oss.str();

ros::Publisher objID_pub;

    // KF init
    int stateDim=4;// [x,y,v_x,v_y]//,w,h]
    int measDim=2;// [z_x,z_y,z_w,z_h]
    int ctrlDim=0;
    cv::KalmanFilter KF0(stateDim,measDim,ctrlDim,CV_32F);
    cv::KalmanFilter KF1(stateDim,measDim,ctrlDim,CV_32F);
    cv::KalmanFilter KF2(stateDim,measDim,ctrlDim,CV_32F);
    cv::KalmanFilter KF3(stateDim,measDim,ctrlDim,CV_32F);
    cv::KalmanFilter KF4(stateDim,measDim,ctrlDim,CV_32F);
    cv::KalmanFilter KF5(stateDim,measDim,ctrlDim,CV_32F);

    ros::Publisher pub_cluster0;
    ros::Publisher pub_cluster1;
    ros::Publisher pub_cluster2;
    ros::Publisher pub_cluster3;
    ros::Publisher pub_cluster4;
    ros::Publisher pub_cluster5;

    ros::Publisher objState0;
    ros::Publisher objState1;
    ros::Publisher objState2;
    ros::Publisher objState3;
    ros::Publisher objState4;
    ros::Publisher objState5;

    ros::Publisher markerPub;
    ros::Publisher textPub;

    std::vector<geometry_msgs::Point> prevClusterCenters;

    cv::Mat state(stateDim,1,CV_32F);
    cv::Mat_<float> measurement(2,1); 

    std::vector<int> objID;// Output of the data association using KF
   // measurement.setTo(Scalar(0));

bool firstFrame=true;

// calculate euclidean distance of two points
  double euclidean_distance(geometry_msgs::Point& p1, geometry_msgs::Point& p2)
    return sqrt((p1.x - p2.x) * (p1.x - p2.x) + (p1.y - p2.y) * (p1.y - p2.y) + (p1.z - p2.z) * (p1.z - p2.z));
//Count unique object IDs. just to make sure same ID has not been assigned to two KF_Trackers.  
int countIDs(vector<int> v)
    transform(v.begin(), v.end(), v.begin(), abs); // O(n) where n = distance(v.end(), v.begin())
    sort(v.begin(), v.end()); // Average case O(n log n), worst case O(n^2) (usually implemented as quicksort.
    // To guarantee worst case O(n log n) replace with make_heap, then sort_heap.

    // Unique will take a sorted range, and move things around to get duplicated
    // items to the back and returns an iterator to the end of the unique section of the range
    auto unique_end = unique(v.begin(), v.end()); // Again n comparisons
    return distance(unique_end, v.begin()); // Constant time for random access iterators (like vector's)


objID: vector containing the IDs of the clusters that should be associated with each KF_Tracker
objID[0] corresponds to KFT0, objID[1] corresponds to KFT1 etc.

std::pair<int,int> findIndexOfMin(std::vector<std::vector<float> > distMat)
    cout<<"findIndexOfMin cALLED\n";
    float minEl=std::numeric_limits<float>::max();
    for (int i=0; i<distMat.size();i++)
        for(int j=0;j<;j ...
edit retag flag offensive close merge delete


( My current guess is something with KF0.statePost, to retrieve the velocity, and then somehow publish it, but im not exactly sure)

th6262 gravatar image th6262  ( 2018-08-12 14:08:10 -0600 )edit

Ive actually managed to publish something:<float>(0)<float>(1)<float>(2)<float>(3)

0 and 1 give me the correct x and y value, but 2 and 3 give me a velocity, which is something around 10^-6, which is too low

th6262 gravatar image th6262  ( 2018-08-12 15:05:30 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2018-08-13 03:36:49 -0600

th6262 gravatar image

I've figured it out! It works with<float>(2) and<float>(3). The result is PIXEL/second. That's why the values are so small! Now I need to figure out a way to convert it to something useful

edit flag offensive delete link more

Question Tools

1 follower


Asked: 2018-08-12 13:31:09 -0600

Seen: 185 times

Last updated: Aug 13 '18