Computed T_pivot_end about latest new added laser in window not used ?

[narutojxl]

Hi @hyye. Thanks for your awesome code first. In initialized phase: We call BuildLocalMap to build local map, from [pivot, end] lasers in window. For the end laser frame, which is new added into window, in ProcessImu use imu meas predict the end frame pose in b0(IMU world frame) frame. When calling CalculateLaserOdom to process the end frame, calculated transform between pivot and end T_pivot_end stored in local_transforms[estimator_config_.window_size] seems not used by something. Because local_transforms is a local variable, after BuildLocalMap return, the local variable is destoried. It is my guess: Should we use T_pivot_end(laser to laser relationship) to refine the predicted frame in b0, and then stored in Ps_[estimator_config_.window_size], Rs_[estimator_config_.window_size] for the following module use? Thanks for your help !

[narutojxl]

Hi @hyye :) I found the commented out code exactlly matches my above guess. I don't figure out for what reason, you comment the following code: from line 1584 to line 1598 in Estimator.cc. If we don't update the imu predicted new added frame pose in b0, Does it mean that there is no need to iteratively call gassian-newton to calculate the T_pivot_end in CalculateLaserOdom() function?

// int pivot_idx = int(estimatorconfig.window_size - estimatorconfig.opt_window_size); // // Twist transform_lb = transformlb.cast(); // // Eigen::Vector3d Pspivot = Ps[pivot_idx]; // Eigen::Matrix3d Rspivot = Rs[pivot_idx]; // // Quaterniond rot_pivot(Rs_pivot transform_lb.rot.inverse()); // Eigen::Vector3d pos_pivot = Ps_pivot - rot_pivot transform_lb.pos; // // Twist transform_pivot = Twist(rot_pivot, pos_pivot); // // Twist transform_bi = transform_pivot local_transforms[idx].cast() transformlb; // Rs[idx] = transformbi.rot.normalized(); // Ps[idx] = transform_bi.pos;