Skip to content

Commit

Permalink
Changed interpolation of location/orientation to use quats (slerp)
Browse files Browse the repository at this point in the history 
While working on a video where the scanner was moving relatively fast (like 10 km/h), differences in interpolation of the LOSolver and lidarscript differed so that origins of the "laser beams" sometimes went outside the lidar unit. Using quat-slerp (or maybe even squads (but not now...)) is also the "right way" to do this, I just were too lazy to do it before stumbling upon problems.

This also affects generating of point clouds and raster cameras. Raster cameras still use the old method when using ITOW-timestamps. May not be a big issue since movement should be minimal when shooting stills.
  • Loading branch information
@GNSS-Stylist
GNSS-Stylist committed Sep 8, 2021
1 parent 4c6e03a commit 01bebfb
Show file tree
Hide file tree
Showing 5 changed files with 206 additions and 70 deletions.
11 changes: 10 additions & 1 deletion PostProcessing/Lidar/lidarscriptgenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,15 @@ void LidarScriptGenerator::generateLidarScript(const Params& params)
RPLidarPlausibilityFilter plausibilityFilter;
plausibilityFilter.setSettings(*params.lidarFilteringSettings);

// Map where uptimes for all equal ITOWs are the same.
// This makes processing later easier
// Uptimes here are calculated as averages from rover values (for each ITOW)
QMap<qint64, UBXMessage_RELPOSNED::ITOW> averagedSync;

emit infoMessage("Generating equalized rover uptime timestamps...");
PostProcessingForm::generateAveragedRoverUptimeSync(params.rovers, averagedSync);
emit infoMessage("Equalized rover uptime timestamps created. Number of items: " + QString::number(averagedSync.size()));

QFile lidarScriptFile;

lidarScriptFile.setFileName(params.fileName);
Expand Down Expand Up @@ -254,7 +263,7 @@ void LidarScriptGenerator::generateLidarScript(const Params& params)

try
{
params.loInterpolator->getInterpolatedLocationOrientationTransformMatrix_Uptime(roverUptime, transform_LoSolver);
params.loInterpolator->getInterpolatedLocationOrientationTransformMatrix_Uptime(roverUptime, averagedSync, transform_LoSolver);
}
catch (QString& stringThrown)
{
Expand Down
11 changes: 10 additions & 1 deletion PostProcessing/Lidar/pointcloudgeneratorlidar.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -335,6 +335,15 @@ bool PointCloudGenerator::generatePointCloudPointSet(const Params& params,

plausibilityFilter.setSettings(*params.lidarFilteringSettings);

// Map where uptimes for all equal ITOWs are the same.
// This makes processing later easier
// Uptimes here are calculated as averages from rover values (for each ITOW)
QMap<qint64, UBXMessage_RELPOSNED::ITOW> averagedSync;

emit infoMessage("Generating equalized rover uptime timestamps...");
PostProcessingForm::generateAveragedRoverUptimeSync(params.rovers, averagedSync);
emit infoMessage("Equalized rover uptime timestamps created. Number of items: " + QString::number(averagedSync.size()));

while ((lidarIter != params.lidarRounds->end()) && (lidarIter.value().startTime < endingUptime))
{
plausibilityFilter.filter(lidarIter.value().distanceItems, filteredItems);
Expand All @@ -360,7 +369,7 @@ bool PointCloudGenerator::generatePointCloudPointSet(const Params& params,

try
{
params.loInterpolator->getInterpolatedLocationOrientationTransformMatrix_Uptime(roverUptime, transform_LoSolver);
params.loInterpolator->getInterpolatedLocationOrientationTransformMatrix_Uptime(roverUptime, averagedSync, transform_LoSolver);
}
catch (QString& stringThrown)
{
Expand Down
225 changes: 162 additions & 63 deletions PostProcessing/postprocessingform.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3158,100 +3158,126 @@ PostProcessingForm::LOInterpolator::LOInterpolator(PostProcessingForm* owner)
{
for (int ii = 0; ii < 2; ii++)
{
roverUptimeLimits[i][ii] = -1;
roverITOWLimits[i][ii] = -1;
}
}
roverUptimeLimit_Low = -1;
roverUptimeLimit_High = -1;
}

void PostProcessingForm::LOInterpolator::getInterpolatedLocationOrientationTransformMatrix_Uptime(const qint64 uptime, Eigen::Transform<double, 3, Eigen::Affine>& transform)
void PostProcessingForm::LOInterpolator::getInterpolatedLocationOrientationTransformMatrix_Uptime(
const qint64 uptime, const QMap<qint64, UBXMessage_RELPOSNED::ITOW>& averagedRoverUptimeSync,
Eigen::Transform<double, 3, Eigen::Affine>& transform,
const unsigned int maxInterpolationTimerange)
{
// TODO: This should use quaternions to work better.
// As measurements are received 8 times/s typically,
// interpolating coordinates may not be a big issue either.
// If using quaternions, rover data should be ITOW-synced.

UBXMessage_RELPOSNED interpolated_Rovers[3];
UBXMessage_RELPOSNED roverRELPOSNEDS_Low[3];
UBXMessage_RELPOSNED roverRELPOSNEDS_High[3];
const Rover* crovers = owner->rovers;

for (int i = 0; i < 3; i++)
if ((uptime < roverUptimeLimit_Low) || (uptime >= roverUptimeLimit_High))
{
if ((uptime <= roverUptimeLimits[i][0]) || (uptime > roverUptimeLimits[i][1]))
// "Cache miss" -> Find new limiting values

auto roverUptimeIter = averagedRoverUptimeSync.upperBound(uptime);

if (roverUptimeIter == averagedRoverUptimeSync.end())
{
// "Cache miss" -> Find new limiting values
roverUptimeLimit_Low = -1;
roverUptimeLimit_High = -1;
throw QString("Can not find corresponding rover uptime-averaged sync data (higher limit).");
}

QMap<qint64, RoverSyncItem>::const_iterator roverUptimeIter = crovers[i].roverSyncData.lowerBound(uptime);
if (roverUptimeIter == averagedRoverUptimeSync.begin())
{
roverUptimeLimit_Low = -1;
roverUptimeLimit_High = -1;
throw QString("Can not find corresponding rover uptime-averaged sync data (lower limit).");
}

if (roverUptimeIter != crovers[i].roverSyncData.end())
{
roverUptimeLimits[i][1] = roverUptimeIter.key();
roverUptimeLimit_High = roverUptimeIter.key();
UBXMessage_RELPOSNED::ITOW roverITOWLimit_High = roverUptimeIter.value();
roverUptimeIter--;
roverUptimeLimit_Low = roverUptimeIter.key();
UBXMessage_RELPOSNED::ITOW roverITOWLimit_Low = roverUptimeIter.value();

const RoverSyncItem upperSyncItem = roverUptimeIter.value();
RoverSyncItem lowerSyncItem;
QMap<UBXMessage_RELPOSNED::ITOW, UBXMessage_RELPOSNED>::const_iterator roverRELPOSNEDS_High[3];
QMap<UBXMessage_RELPOSNED::ITOW, UBXMessage_RELPOSNED>::const_iterator roverRELPOSNEDS_Low[3];

if (roverUptimeIter == crovers[i].roverSyncData.begin())
{
roverUptimeLimits[i][0] = -1;
roverUptimeLimits[i][1] = -1;
throw QString("Can not find corresponding rover" + owner->getRoverIdentString(i) + " sync data (higher limit).");
}
for (int i = 0; i < 3; i++)
{
roverRELPOSNEDS_Low[i] = crovers[i].relposnedMessages.lowerBound(roverITOWLimit_Low);
roverRELPOSNEDS_High[i] = crovers[i].relposnedMessages.lowerBound(roverITOWLimit_High);
Q_ASSERT(roverRELPOSNEDS_Low[i].key() == roverITOWLimit_Low);
Q_ASSERT(roverRELPOSNEDS_High[i].key() == roverITOWLimit_High);
}

roverUptimeIter--;
lowerSyncItem = roverUptimeIter.value();
roverUptimeLimits[i][0] = roverUptimeIter.key();
Eigen::Vector3d points_Low[3] =
{
{ roverRELPOSNEDS_Low[0]->relPosN, roverRELPOSNEDS_Low[0]->relPosE, roverRELPOSNEDS_Low[0]->relPosD },
{ roverRELPOSNEDS_Low[1]->relPosN, roverRELPOSNEDS_Low[1]->relPosE, roverRELPOSNEDS_Low[1]->relPosD },
{ roverRELPOSNEDS_Low[2]->relPosN, roverRELPOSNEDS_Low[2]->relPosE, roverRELPOSNEDS_Low[2]->relPosD },
};

if (crovers[i].relposnedMessages.find(upperSyncItem.iTOW) == crovers[i].relposnedMessages.end())
{
roverUptimeLimits[i][0] = -1;
roverUptimeLimits[i][1] = -1;
throw QString("Can not find corresponding rover" + owner->getRoverIdentString(i) + " iTOW (higher limit).");
}
if (!loSolver.setPoints(points_Low))
{
throw QString("LOSolver.setPoints (low limit) failed. Error code: " + QString::number(loSolver.getLastError()) + ".");
}

if (crovers[i].relposnedMessages.find(lowerSyncItem.iTOW) == crovers[i].relposnedMessages.end())
{
roverUptimeLimits[i][0] = -1;
roverUptimeLimits[i][1] = -1;
throw QString("Can not find corresponding rover" + owner->getRoverIdentString(i) + " iTOW (lower limit).");
}
Eigen::Transform<double, 3, Eigen::Affine> transform_Low;

roverUptimeBasedRELPOSNEDS_Lower[i] = crovers[i].relposnedMessages.find(lowerSyncItem.iTOW).value();
roverUptimeBasedRELPOSNEDS_Upper[i] = crovers[i].relposnedMessages.find(upperSyncItem.iTOW).value();
}
else
{
roverUptimeLimits[i][0] = -1;
roverUptimeLimits[i][1] = -1;
throw QString("Can not find corresponding rover" + owner->getRoverIdentString(i) + " sync data (upper limit).");
}
if (!loSolver.getTransformMatrix(transform_Low))
{
throw QString("LOSolver.getTransformMatrix failed. Error code: " + QString::number(loSolver.getLastError()) + ".");
}

qint64 timeDiff = uptime - roverUptimeLimits[i][0];
roverUptimeBasedLocation_Low = transform_Low.translation();
roverUptimeBasedOrientation_Low = transform_Low.linear();

interpolated_Rovers[i] = UBXMessage_RELPOSNED::interpolateCoordinates(roverUptimeBasedRELPOSNEDS_Lower[i],
roverUptimeBasedRELPOSNEDS_Upper[i], roverUptimeBasedRELPOSNEDS_Lower[i].iTOW + timeDiff);
}
Eigen::Vector3d points_High[3] =
{
{ roverRELPOSNEDS_High[0]->relPosN, roverRELPOSNEDS_High[0]->relPosE, roverRELPOSNEDS_High[0]->relPosD },
{ roverRELPOSNEDS_High[1]->relPosN, roverRELPOSNEDS_High[1]->relPosE, roverRELPOSNEDS_High[1]->relPosD },
{ roverRELPOSNEDS_High[2]->relPosN, roverRELPOSNEDS_High[2]->relPosE, roverRELPOSNEDS_High[2]->relPosD },
};

Eigen::Vector3d points[3] =
{
{ interpolated_Rovers[0].relPosN, interpolated_Rovers[0].relPosE, interpolated_Rovers[0].relPosD },
{ interpolated_Rovers[1].relPosN, interpolated_Rovers[1].relPosE, interpolated_Rovers[1].relPosD },
{ interpolated_Rovers[2].relPosN, interpolated_Rovers[2].relPosE, interpolated_Rovers[2].relPosD },
};
if (!loSolver.setPoints(points_High))
{
throw QString("LOSolver.setPoints (high limit) failed. Error code: " + QString::number(loSolver.getLastError()) + ".");
}

if (!loSolver.setPoints(points))
{
throw QString("LOSolver.setPoints failed. Error code: " + QString::number(loSolver.getLastError()) + ".");
Eigen::Transform<double, 3, Eigen::Affine> transform_High;

if (!loSolver.getTransformMatrix(transform_High))
{
throw QString("LOSolver.getTransformMatrix failed. Error code: " + QString::number(loSolver.getLastError()) + ".");
}

roverUptimeBasedLocation_High = transform_High.translation();
roverUptimeBasedOrientation_High = transform_High.linear();
}

if (!loSolver.getTransformMatrix(transform))
if (roverUptimeLimit_High - roverUptimeLimit_Low > maxInterpolationTimerange)
{
throw QString("LOSolver.getTransformMatrix failed. Error code: " + QString::number(loSolver.getLastError()) + ".");
throw("Maximum allowed time (" + QString::number(maxInterpolationTimerange) +
"ms) for interpolation exceeded. (Interpolation time: " +
QString::number(roverUptimeLimit_High - roverUptimeLimit_Low) + ").");
}

double fraction = double(uptime - roverUptimeLimit_Low) / (roverUptimeLimit_High - roverUptimeLimit_Low);

Q_ASSERT(fraction >= 0);
Q_ASSERT(fraction <= 1);

Eigen::Quaterniond slerpedQuat = roverUptimeBasedOrientation_Low.slerp(fraction, roverUptimeBasedOrientation_High);
Eigen::Vector3d interpolatedCoords = roverUptimeBasedLocation_Low + fraction * (roverUptimeBasedLocation_High - roverUptimeBasedLocation_Low);

transform.linear() = slerpedQuat.toRotationMatrix();
transform.translation() = interpolatedCoords;
}

void PostProcessingForm::LOInterpolator::getInterpolatedLocationOrientationTransformMatrix_ITOW(const UBXMessage_RELPOSNED::ITOW iTOW, Eigen::Transform<double, 3, Eigen::Affine>& transform)
{
// TODO: This should use quaternions to work better.
// TODO: This should use quaternions to work better (see the uptime-version above).
// As measurements are received 8 times/s typically,
// interpolating coordinates may not be a big issue either.
// If using quaternions, rover data should be ITOW-synced.
Expand Down Expand Up @@ -3917,3 +3943,76 @@ void PostProcessingForm::on_pushButton_RasterCameras_Script_Save_clicked()
}
}

void PostProcessingForm::generateAveragedRoverUptimeSync(const Rover* rovers, QMap<qint64, UBXMessage_RELPOSNED::ITOW>& averagedRoverUptimeSync, unsigned int numOfAveragedRovers)
{
// Create a map where uptimes for all equal ITOWs are the same.
// This makes processing (like interpolating locations/orientations) easier.
// Uptimes here are calculated as averages from rover values (for each ITOW)

averagedRoverUptimeSync.clear();

Q_ASSERT(numOfAveragedRovers <= 3);

for (unsigned int i = 0; i < numOfAveragedRovers; i++)
{
if (rovers[i].roverSyncData.isEmpty())
{
// Nothing to do
return;
}
}

qint64 currentITOW = rovers[0].reverseSync.firstKey();

QMap<UBXMessage_RELPOSNED::ITOW, qint64>::const_iterator iTOWIterators[3];

while (1)
{
for (unsigned int i = 0; i < numOfAveragedRovers; i++)
{
iTOWIterators[i] = rovers[i].reverseSync.lowerBound(currentITOW);
}

bool dataLeft = false;

for (unsigned int i = 0; i < numOfAveragedRovers; i++)
{
if (iTOWIterators[i] != rovers[i].reverseSync.end())
{
dataLeft = true;
break;
}
}

if (!dataLeft)
{
break;
}

qint64 uptimeSum = 0;
unsigned int numberOfRoverRELPOSNEDsWithSameITOW = 0;

for (unsigned int i = 0; i < numOfAveragedRovers; i++)
{
if ((iTOWIterators[i] != rovers[i].reverseSync.end()) &&
(iTOWIterators[i].key() == currentITOW))
{
uptimeSum += iTOWIterators[i].value();
numberOfRoverRELPOSNEDsWithSameITOW++;
}
else
{
// Only allow rovers A, AB or ABC (not AC, BC etc)
break;
}
}

if (numberOfRoverRELPOSNEDsWithSameITOW >= numOfAveragedRovers)
{
averagedRoverUptimeSync[uptimeSum / numberOfRoverRELPOSNEDsWithSameITOW] = currentITOW;
}

currentITOW++;
}
}

18 changes: 14 additions & 4 deletions PostProcessing/postprocessingform.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -130,16 +130,25 @@ class PostProcessingForm : public QWidget
{
public:
LOInterpolator(PostProcessingForm* owner);
void getInterpolatedLocationOrientationTransformMatrix_Uptime(const qint64 uptime, Eigen::Transform<double, 3, Eigen::Affine>& transform);

void getInterpolatedLocationOrientationTransformMatrix_Uptime(
const qint64 uptime, const QMap<qint64, UBXMessage_RELPOSNED::ITOW>& averagedRoverUptimeSync,
Eigen::Transform<double, 3, Eigen::Affine>& transform,
const unsigned int maxInterpolationTimeRange = 500);

void getInterpolatedLocationOrientationTransformMatrix_ITOW(const UBXMessage_RELPOSNED::ITOW iTOW, Eigen::Transform<double, 3, Eigen::Affine>& transform);

LOSolver loSolver; // This must be initialized by user of this class before using the interpolation function!

private:
PostProcessingForm* owner = nullptr;
qint64 roverUptimeLimits[3][2];
UBXMessage_RELPOSNED roverUptimeBasedRELPOSNEDS_Lower[3];
UBXMessage_RELPOSNED roverUptimeBasedRELPOSNEDS_Upper[3];

qint64 roverUptimeLimit_Low = -1;
qint64 roverUptimeLimit_High = -1;
Eigen::Vector3d roverUptimeBasedLocation_Low;
Eigen::Vector3d roverUptimeBasedLocation_High;
Eigen::Quaterniond roverUptimeBasedOrientation_Low;
Eigen::Quaterniond roverUptimeBasedOrientation_High;

UBXMessage_RELPOSNED::ITOW roverITOWLimits[3][2];
UBXMessage_RELPOSNED roverITOWBasedRELPOSNEDS_Lower[3];
Expand All @@ -151,6 +160,7 @@ class PostProcessingForm : public QWidget
~PostProcessingForm();

static QString getRoverIdentString(const unsigned int roverId);
static void generateAveragedRoverUptimeSync(const Rover* rovers, QMap<qint64, UBXMessage_RELPOSNED::ITOW> &averagedRoverUptimeSync, unsigned int numOfAveragedRovers = 3);

protected:
void showEvent(QShowEvent* event); //!< Initializes some things that can't be initialized in constructor
Expand Down
11 changes: 10 additions & 1 deletion PostProcessing/rastercameragenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -532,6 +532,15 @@ void RasterCameraGenerator::cmd_ProcessStills(const QVector<Item>& command, cons

QStringList fileList = dir.entryList();

// Map where uptimes for all equal ITOWs are the same.
// This makes processing later easier
// Uptimes here are calculated as averages from rover values (for each ITOW)
QMap<qint64, UBXMessage_RELPOSNED::ITOW> averagedSync;

emit infoMessage("Generating equalized rover uptime timestamps...");
PostProcessingForm::generateAveragedRoverUptimeSync(params.rovers, averagedSync);
emit infoMessage("Equalized rover uptime timestamps created. Number of items: " + QString::number(averagedSync.size()));

for (int i = 0; i < fileList.size(); i++)
{
QString fileName = fullPath(fileList[i]);
Expand Down Expand Up @@ -562,7 +571,7 @@ void RasterCameraGenerator::cmd_ProcessStills(const QVector<Item>& command, cons

try
{
params.loInterpolator->getInterpolatedLocationOrientationTransformMatrix_Uptime(imageUptime, transform_LoSolver);
params.loInterpolator->getInterpolatedLocationOrientationTransformMatrix_Uptime(imageUptime, averagedSync, transform_LoSolver);
}
catch (QString& stringThrown)
{
Expand Down

1 comment on commit 01bebfb

@GNSS-Stylist
Copy link
Owner Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Mentioned ITOW-based camera interpolation changed in commit cd62cf7

Please sign in to comment.