search

HITSZ-NRSL / Dynamic-VINS

[RA-L 2022] RGB-D Inertial Odometry for a Resource-restricted Robot in Dynamic Environments
310 stars 40 forks source link

The weighted predicted velocity #19

Closed HJMGARMIN closed 8 months ago

HJMGARMIN commented 1 year ago

Hello, I have a new question about the weighted predicted velocity in Eq.(4) of your paper. image As we can see, the weighted predicted velocity of j+1 frame is updated by the weigthed predicted velocity and pixel velocity of j frame. In the source code, I found that the weighted predicted velocity of j frame is same as the pixel velocity.

if (temp_object_id > 0) { dynamic_objects[temp_object_id].x_vel = x_center - dynamic_objects[temp_object_id].x_center; dynamic_objects[temp_object_id].y_vel = y_center - dynamic_objects[temp_object_id].y_center;

        dynamic_objects[temp_object_id].x_weight_vel = dynamic_objects[temp_object_id].x_vel;
        dynamic_objects[temp_object_id].y_weight_vel = dynamic_objects[temp_object_id].y_vel;
    }
    else
    {
        temp_object_id = ++object_id;
    }

    dynamic_objects[temp_object_id].x_center = x_center;
    dynamic_objects[temp_object_id].y_center = y_center;
    dynamic_objects[temp_object_id].x1       = x1;
    dynamic_objects[temp_object_id].y1       = y1;
    dynamic_objects[temp_object_id].x2       = x2;
    dynamic_objects[temp_object_id].y2       = y2;

    dynamic_objects[temp_object_id].x_weight_vel =
        (dynamic_objects[temp_object_id].x_weight_vel + dynamic_objects[temp_object_id].x_vel) /
        2;
    dynamic_objects[temp_object_id].y_weight_vel =
        (dynamic_objects[temp_object_id].y_weight_vel + dynamic_objects[temp_object_id].y_vel) /
        2;

Firstly, compute the pixel velocity dynamic_objects[temp_object_id].x_vel of the box in j frame. Secondly, update dynamic_objects[temp_object_id].x_weight_vel using dynamic_objects[temp_object_id].x_vel. Now, the pixel velocity and the weighted predicted velocity are same. Then, dynamic_objects[temp_object_id].x_weight_vel = (dynamic_objects[temp_object_id].x_weight_vel + dynamic_objects[temp_object_id].x_vel) / 2; seems just like x_weight_vel = (x_vel+x_vel)/2=x_vel. It seems does not change the value of x_weight_vel compared to dynamic_objects[temp_object_id].x_weight_vel = dynamic_objects[temp_object_id].x_vel;. So, what is the function of the code? I am so confused about it.

jianhengLiu commented 1 year ago

So sorry that I have missed this issue. It seems that I added codes mistakenly while refactoring. The correct codes should be:

if (!is_compensate)
    {
        unsigned short temp_object_id = prev_semantic_id.at<unsigned short>(y_center, x_center);

        if (temp_object_id > 0)
        {
            dynamic_objects[temp_object_id].x_vel = x_center - dynamic_objects[temp_object_id].x_center;
            dynamic_objects[temp_object_id].y_vel = y_center - dynamic_objects[temp_object_id].y_center;
        }
        else
        {
            temp_object_id = ++object_id;
        }

        dynamic_objects[temp_object_id].x_center = x_center;
        dynamic_objects[temp_object_id].y_center = y_center;
        dynamic_objects[temp_object_id].x1 = x1;
        dynamic_objects[temp_object_id].y1 = y1;
        dynamic_objects[temp_object_id].x2 = x2;
        dynamic_objects[temp_object_id].y2 = y2;

        dynamic_objects[temp_object_id].x_weight_vel =
            (dynamic_objects[temp_object_id].x_weight_vel + dynamic_objects[temp_object_id].x_vel) / 2;
        dynamic_objects[temp_object_id].y_weight_vel =
            (dynamic_objects[temp_object_id].y_weight_vel + dynamic_objects[temp_object_id].y_vel) / 2;
        dynamic_objects[temp_object_id].is_update = true;
        dynamic_objects[temp_object_id].no_update_times = 0;
        cv::rectangle(semantic_id, cv::Point(x1, y1), cv::Point(x2, y2), cv::Scalar(temp_object_id), -1);
    }