diff --git a/corelib/src/Odometry.cpp b/corelib/src/Odometry.cpp index dd9b35b4..ad27eafd 100644 --- a/corelib/src/Odometry.cpp +++ b/corelib/src/Odometry.cpp @@ -779,6 +779,24 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet } + // Features that came with the frame are placed in the full size image, while what + // is about to be registered is the decimated one and the calibration that goes + // with it, so bring them along. They are scaled back below with whatever the + // registration returns, leaving the caller its own frame of reference. + if(!decimatedData.keypoints().empty()) + { + std::vector decimatedKpts = decimatedData.keypoints(); + double log2value = log(double(_imageDecimation))/log(2.0); + for(unsigned int i=0; icomputeTransform(decimatedData, guess, info);