Transform a point - problem on rotation

edit

Hello,

I have a point on base_link (robot frame). When the robot move I want to compensate the point position so it stays on the same position (odom). Per example at t=0 the point is at 0,0 (base_link). At t=1 the robot is at 1,0 (odom) so my point should be at -1,0 (base_link). Suppose the odometry is perfect.

https://youtu.be/K0w0yqEq44g

As you can see the two squares are moving okay until the robot start to rotate then my code start moving the squares on the wrong way.

How I'm trying to do it:

void odometryCallback(const nav_msgs::Odometry& msg){

    static tf::TransformBroadcaster br;

    tf::StampedTransform transform;
    try{
        listener->lookupTransform("odom", "base_link",
                                 ros::Time(0), transform);
    }
    catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
        return;
    }

    double delta_x = transform.getOrigin().x() - last_transform.getOrigin().x();
    double delta_y = transform.getOrigin().y() - last_transform.getOrigin().y();
    double delta_theta = tf::getYaw(transform.getRotation())-tf::getYaw(last_transform.getRotation());

    last_transform = transform;

    if(std::isnan(delta_theta))
        return;

    convertArrayToPointCloud();
    for(int i=0;i<point_cloud_transformed.points.size();i++){
        point_cloud_transformed.points.at(i) = rotMat(point_cloud_transformed.points.at(i),-delta_theta);
        point_cloud_transformed.points.at(i).x = point_cloud_transformed.points.at(i).x + delta_x*cos(delta_theta);
        point_cloud_transformed.points.at(i).y = point_cloud_transformed.points.at(i).y - delta_y*sin(delta_theta);
    }

    pub_cloud.publish(point_cloud_transformed);

    pointCloudToArray();

}

It works on some directions only, so I suppose I'm doing something wrong about the delta_theta but can't find what :s