Эффективность памяти — Eigen :: VectorXd в цикле

у меня есть Measurement объект, который имеет два Eigen::VectorXd участники — один для position и другие velocity,

Измерения упорядочены в наборе данных с помощью сканирований, то есть на каждом временном шаге в набор данных добавляется новое сканирование измерений. Эти типы определены как:

typedef std::shared_ptr<Measurement>        MeasurementPtr;
typedef std::vector<MeasurementPtr>         scan_t;
typedef std::vector<scan_t>                 dataset_t;

В начале каждой итерации моего алгоритма мне нужно применить новое преобразование к каждому измерению. В настоящее время у меня есть:

for (auto scan = dataset_.begin(); scan != dataset_.end(); ++scan)
for (auto meas = scan->begin(); meas != scan->end(); ++meas) {

// Transform this measurement to bring it into the same
// coordinate frame as the current scan
if (scan != std::prev(dataset_.end())) {
core::utils::perspective_transform(T_, (*meas)->pos);
core::utils::perspective_transform(T_, (*meas)->vel);
}

}

куда perspective_transform определяется как

void perspective_transform(const Eigen::Projective2d& T, Eigen::VectorXd& pos) {

pos = (T*pos.homogeneous()).hnormalized();
}

Добавление этого кода увеличивает время вычислений в 40 раз, когда я запускаю алгоритм со сканированием в наборе данных с 50 измерениями в каждом сканировании, что делает его довольно медленным. Я считаю, что это потому, что у меня есть 550 маленьких объектов, каждый с 2 Eigen память пишет. Я удалил запись результата в память, и мой тест показывает лишь небольшое уменьшение — предполагая, что это проблема эффективности памяти, а не узкое место вычислений.

Как я могу ускорить это вычисление? Есть ли способ сначала пройти через и создать Eigen::Matrix от Eigen::Map что я мог бы тогда сделать вычисление один раз и автоматически обновить два члена всех Measurement объекты?