Feature/backwards eigen bounds #219
Conversation
experimental/beacon_sim/BUILD
Outdated
| ":test_helpers", | ||
| "@com_google_googletest//:gtest_main", | ||
| ] | ||
| ) |
There was a problem hiding this comment.
could we remove the planner changes from this PR?
| const double lower_eigen_value_information, // min ev of \Omega_t | ||
| const double upper_eigen_value_dynamics, // max ev of G_t^{-1} G_t^{-T} | ||
| const double upper_eigen_value_measurement, // max ev of M_t | ||
| const double lower_eigen_value_process_noise // min ev of R_t |
There was a problem hiding this comment.
When you have a bunch of parameters that are the same time, it becomes very easy to accidentally pass the wrong argument into the incorrect location. Consider instead:
struct StepLowerEigenBoundInputs {
double lower_eigen_value_information;
double upper_eigen_value_dynamics;
double upper_eigen_value_measurement;
double lower_eigen_value_process_noise;
};
double step_lower_eigen_bound(const StepLowerEigenBoundInputs &in) { ... }
Then at the call site:
step_lower_eigen_bound({
.lower_eigen_value_information = ...,
.upper_eigen_value_dynamics = ...,
.upper_eigen_value_measurement = ...,
.lower_eigen_value_process_noise = ...,
});
| // G_t^{-1} | ||
| const Eigen::Matrix3d inv_dynamics_jac_wrt_state = old_robot_from_new_robot.Adj(); | ||
| // max_eigen_value(G_t^{-1} G_t^{-T}) | ||
| const double dynamics_upper_eigen_value_bound = (inv_dynamics_jac_wrt_state * inv_dynamics_jac_wrt_state.transpose()).eigenvalues().real().maxCoeff(); |
There was a problem hiding this comment.
this isn't actually a bound is it? it's the true max eigenvalue.
| std::cout << "inv_dynamics_jac_wrt_state: " << std::endl << inv_dynamics_jac_wrt_state << std::endl; | ||
| std::cout << "dynamics_upper_eigen_value_bound: " << dynamics_upper_eigen_value_bound << std::endl; |
There was a problem hiding this comment.
omit when done debugging.
| std::cout << "inv_dynamics_jac_wrt_state: " << std::endl << inv_dynamics_jac_wrt_state << std::endl; | ||
| std::cout << "dynamics_upper_eigen_value_bound: " << dynamics_upper_eigen_value_bound << std::endl; | ||
| // R_t | ||
| const Eigen::Matrix3d process_noise = compute_process_noise(ekf_config, DT_S, old_robot_from_new_robot.arclength()); |
There was a problem hiding this comment.
Perhaps this should return an Eigen::DiagonalMatrix so that the line below is always true.
| // scattering_transform = (scattering_transform * process_transform).value(); | ||
| // scattering_transform = (scattering_transform * measurement_transform).value(); |
| const double lower_eigen_value_process_noise // min ev of R_t | ||
| ); | ||
|
|
||
| double compute_backwards_edge_belief_transform( |
There was a problem hiding this comment.
we should probably call this function something else.
| ); | ||
| std::cout << "information_lower_bound: " << information_lower_bound << std::endl; | ||
| } | ||
| return information_lower_bound; |
There was a problem hiding this comment.
I think this will work. If for some reason we run into computational issues, it might be interesting to compute the aggregate transform per edge and perform the backwards update in one shot.
| constexpr double start_info_eigen_value = 1.0; | ||
| constexpr double ub_dynamics_eigen_value = 1.0; | ||
| constexpr double ub_measurement_eigen_value = 0.0; | ||
| constexpr double lower_process_noise_eigen_value = 1.0; |
There was a problem hiding this comment.
Consider making your test values something other than 0.0 or 1.0, unless you are specifically testing behavior at those values. Adding 0.0 or multiplying by 1.0 are additive and multiplicative identities, so bugs tend to sneak through if you fail to add/multiply something or if you mistakenly do so.
| constexpr double start_info_eigen_value = 1.0; | ||
| constexpr double ub_dynamics_eigen_value = 1.0; | ||
| constexpr double ub_measurement_eigen_value = 1.0; | ||
| constexpr double lower_process_noise_eigen_value = 1.0; |
|
Removing |
… edges. tests run but full edge propogation stil wip
efahnestock
force-pushed
the
feature/backwards_eigen_bounds
branch
from
d45bb53 to
4e3ab8a
Compare
|
changes should address comments! let me know what's next for this |
|
Looks good! |
No description provided.