-
Notifications
You must be signed in to change notification settings - Fork 615
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Additional dynamic init thresholds #330
Conversation
@@ -909,6 +909,15 @@ bool DynamicInitializer::initialize(double ×tamp, Eigen::MatrixXd &covarian | |||
return state_imu; | |||
}; | |||
|
|||
// Calculate the distance between the head and the tail. | |||
// If the movement is smaller than threshold, the parallax is not enough and initialization is not allowed | |||
Eigen::Vector3d movement = get_pose(newest_cam_time).segment<3>(4) - get_pose(oldest_camera_time).segment<3>(4); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please use .block(4, 0, 3, 1)
here.
Eigen::Vector3d movement = get_pose(newest_cam_time).segment<3>(4) - get_pose(oldest_camera_time).segment<3>(4); | ||
double distance = movement.norm(); | ||
if (distance < params.init_dyn_min_movement) { | ||
PRINT_DEBUG(YELLOW "[init-d]: the distance is %.3f < %.3f, intialization failed\n" RESET, distance, params.init_dyn_min_movement); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Make this a warning level. E.g.:
PRINT_WARNING(YELLOW "[init-d]: xxxxxxx!\n" RESET);
Also fix the spelling.
Should also likely check if the velocity is near zero too as we should likely use static in this case. |
I add a flag to control the minimum velocity for dynamic initialization, and I suggest to use two parameters to control the window length for static and dynamic initialization respectively |
I have thought about this, and will close this feature for now. Maybe will revisit in the future if the dynamic initialization module is cleaned up. |
add init_dyn_min_movement flag to make dynamic intialization more robust