Register internal stages timestamps

This commit is contained in:
Mateo de Mayo 2022-05-06 15:30:37 -03:00
parent e66ed19bf9
commit c2f052f9f8
5 changed files with 31 additions and 4 deletions

View File

@ -103,7 +103,7 @@ class FrameToFrameOpticalFlow : public OpticalFlowBase {
if (output_queue) output_queue->push(nullptr);
break;
}
input_ptr->addTime("opticalflow_received");
input_ptr->addTime("frames_received");
processFrame(input_ptr->t_ns, input_ptr);
}

View File

@ -34,6 +34,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#pragma once
#include <chrono>
#include <memory>
#include <Eigen/Geometry>
@ -56,6 +57,22 @@ struct OpticalFlowInput {
int64_t t_ns;
std::vector<ImageData> img_data;
// Keep track of internal timestamps for this input
bool timing_enabled = false;
std::vector<int64_t> tss{};
void addTime(const char* /* name */, int64_t custom_ts = INT64_MIN) {
if (!timing_enabled) {
return;
}
if (custom_ts != INT64_MIN) {
tss.push_back(custom_ts);
} else {
auto ts = std::chrono::steady_clock::now().time_since_epoch().count();
tss.push_back(ts);
}
}
};
struct OpticalFlowResult {

View File

@ -120,7 +120,8 @@ class SqrtKeypointVioEstimator : public VioEstimatorBase,
// int64_t propagate();
// void addNewState(int64_t data_t_ns);
void optimize_and_marg(const std::map<int64_t, int>& num_points_connected,
void optimize_and_marg(const OpticalFlowInput::Ptr& input_images,
const std::map<int64_t, int>& num_points_connected,
const std::unordered_set<KeypointId>& lost_landmaks);
void marginalize(const std::map<int64_t, int>& num_points_connected,

View File

@ -175,6 +175,7 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
if (!curr_frame.get()) {
break;
}
curr_frame->input_images->addTime("vio_start");
// Correct camera time offset
// curr_frame->t_ns += calib.cam_time_offset_ns;
@ -252,6 +253,7 @@ void SqrtKeypointVioEstimator<Scalar_>::initialize(const Eigen::Vector3d& bg_,
data->t_ns = tmp;
}
}
curr_frame->input_images->addTime("imu_preintegrated");
measure(curr_frame, meas);
prev_frame = curr_frame;
@ -477,8 +479,10 @@ bool SqrtKeypointVioEstimator<Scalar_>::measure(
}
}
}
opt_flow_meas->input_images->addTime("landmarks_updated");
optimize_and_marg(num_points_connected, lost_landmaks);
optimize_and_marg(opt_flow_meas->input_images, num_points_connected,
lost_landmaks);
if (out_state_queue) {
PoseVelBiasStateWithLin p = frame_states.at(last_state_t_ns);
@ -486,6 +490,8 @@ bool SqrtKeypointVioEstimator<Scalar_>::measure(
typename PoseVelBiasState<double>::Ptr data(
new PoseVelBiasState<double>(p.getState().template cast<double>()));
data->input_images = opt_flow_meas->input_images;
data->input_images->addTime("pose_produced");
out_state_queue->push(data);
}
@ -1425,10 +1431,13 @@ void SqrtKeypointVioEstimator<Scalar_>::optimize() {
template <class Scalar_>
void SqrtKeypointVioEstimator<Scalar_>::optimize_and_marg(
const OpticalFlowInput::Ptr& input_images,
const std::map<int64_t, int>& num_points_connected,
const std::unordered_set<KeypointId>& lost_landmaks) {
optimize();
input_images->addTime("optimized");
marginalize(num_points_connected, lost_landmaks);
input_images->addTime("marginalized");
}
template <class Scalar_>

@ -1 +1 @@
Subproject commit 8c39d10b3d5463bf257701a32293caee088b8836
Subproject commit 1473e8a417313082372bd0356c732cfcd60e183e