mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-03 16:47:47 +08:00
Added robust graph optimization integration test
This commit is contained in:
matlabbe
@@ -70,6 +70,7 @@ struct ReplayResult
|
||||
int odomNonNull = 0; // odometry returned a non-null pose
|
||||
int odomLost = 0; // odometry returned a null pose
|
||||
int loopClosuresAccepted = 0;
|
||||
int loopClosuresRejected = 0; // sum of frames with Loop/RejectedHypothesis=1
|
||||
int proximityDetections = 0;
|
||||
Transform lastOdomPose;
|
||||
// Latest Gt/translational_rmse from Rtabmap statistics (negative = never
|
||||
@@ -406,6 +407,116 @@ ReplayResult replayDatabase(
|
||||
return result;
|
||||
}
|
||||
|
||||
// Replay variant that feeds the stored odometry pose straight to Rtabmap
|
||||
// without re-running an Odometry stage. Used for datasets where the DB
|
||||
// already carries a known-good odom track and the test only cares about
|
||||
// the back-end (loop closure detection + graph optimization).
|
||||
ReplayResult replayDatabaseWithStoredOdom(
|
||||
const std::string & dbPath,
|
||||
const std::string & workDb,
|
||||
const ParametersMap & rtabmapParameters)
|
||||
{
|
||||
ReplayResult result;
|
||||
|
||||
// odometryIgnored=false so DBReader populates info.odomPose.
|
||||
// featuresIgnored=false so DBReader replays the stored keypoints /
|
||||
// descriptors instead of forcing Rtabmap to re-extract — paired with
|
||||
// Mem/UseOdomFeatures=true in the per-variant parameters so Rtabmap
|
||||
// actually reuses them rather than recomputing on every frame.
|
||||
DBReader dbReader(
|
||||
dbPath,
|
||||
0.0f, // frameRate: 0 = process as fast as possible
|
||||
false, // odometryIgnored
|
||||
true, // ignoreGoalDelay
|
||||
true, // goalsIgnored
|
||||
0, // startId
|
||||
std::vector<unsigned int>(), // cameraIndices
|
||||
0, // stopId
|
||||
false, // intermediateNodesIgnored
|
||||
false, // landmarksIgnored
|
||||
false); // featuresIgnored
|
||||
if(!dbReader.init())
|
||||
{
|
||||
ADD_FAILURE() << "Failed to init DBReader for " << dbPath;
|
||||
return result;
|
||||
}
|
||||
|
||||
UFile::erase(workDb);
|
||||
Rtabmap rtabmap;
|
||||
rtabmap.init(rtabmapParameters, workDb);
|
||||
std::cout << "[ ] Output DB: " << workDb << std::endl;
|
||||
|
||||
SensorCaptureInfo info;
|
||||
SensorData data = dbReader.takeData(&info);
|
||||
while(data.isValid())
|
||||
{
|
||||
++result.framesRead;
|
||||
// In this replay mode the DB is expected to carry a stored odom
|
||||
// pose and covariance on every frame; treat either being missing
|
||||
// as a malformed asset rather than silently skipping.
|
||||
if(info.odomPose.isNull())
|
||||
{
|
||||
ADD_FAILURE() << "Frame " << result.framesRead
|
||||
<< " has no stored odometry pose";
|
||||
return result;
|
||||
}
|
||||
if(info.odomCovariance.empty() || info.odomCovariance.total() != 36)
|
||||
{
|
||||
ADD_FAILURE() << "Frame " << result.framesRead
|
||||
<< " has no stored odometry covariance";
|
||||
return result;
|
||||
}
|
||||
result.lastOdomPose = info.odomPose;
|
||||
|
||||
rtabmap.process(data, info.odomPose, info.odomCovariance);
|
||||
++result.framesProcessed;
|
||||
const Statistics & stats = rtabmap.getStatistics();
|
||||
if(stats.loopClosureId() > 0)
|
||||
{
|
||||
++result.loopClosuresAccepted;
|
||||
}
|
||||
if(stats.proximityDetectionId() > 0)
|
||||
{
|
||||
++result.proximityDetections;
|
||||
}
|
||||
// Loop/RejectedHypothesis is published as 1.0f on frames where a
|
||||
// loop closure (or proximity link) was discarded — either by the
|
||||
// addLink path or by the RGBD/OptimizeMaxError post-optimization
|
||||
// rejection. Robust + MaxError-enabled variants are expected to
|
||||
// reject more than the unguarded variants.
|
||||
const auto rejIt = stats.data().find(Statistics::kLoopRejectedHypothesis());
|
||||
if(rejIt != stats.data().end() && rejIt->second > 0.5f)
|
||||
{
|
||||
++result.loopClosuresRejected;
|
||||
}
|
||||
data = dbReader.takeData(&info);
|
||||
}
|
||||
|
||||
{
|
||||
std::multimap<int, Link> constraints;
|
||||
rtabmap.getGraph(result.finalLocalPoses, constraints,
|
||||
/*optimized=*/true, /*global=*/false);
|
||||
result.finalLocalGraphSize = (int)result.finalLocalPoses.size();
|
||||
constraints.clear();
|
||||
rtabmap.getGraph(result.finalGlobalPoses, constraints,
|
||||
/*optimized=*/true, /*global=*/true);
|
||||
result.finalGlobalGraphSize = (int)result.finalGlobalPoses.size();
|
||||
}
|
||||
|
||||
std::cout << "[ ] Replay summary:"
|
||||
<< " framesRead=" << result.framesRead
|
||||
<< " framesProcessed=" << result.framesProcessed
|
||||
<< " loops=" << result.loopClosuresAccepted
|
||||
<< " loopsRejected=" << result.loopClosuresRejected
|
||||
<< " proximity=" << result.proximityDetections
|
||||
<< " localGraph=" << result.finalLocalGraphSize
|
||||
<< " globalGraph=" << result.finalGlobalGraphSize
|
||||
<< std::endl;
|
||||
|
||||
rtabmap.close(true);
|
||||
return result;
|
||||
}
|
||||
|
||||
ParametersMap baseRtabmapParams()
|
||||
{
|
||||
ParametersMap params;
|
||||
@@ -888,3 +999,129 @@ TEST_F(RtabmapIntegrationFixture, TwoLoopsWorkspaceGlobalBA)
|
||||
}
|
||||
ASSERT_GT(variantsTested, 0) << "no BA-capable optimizer was available";
|
||||
}
|
||||
|
||||
// ---------------------------------------------------------------------------
|
||||
// Robust-graph-optimization tutorial DB (stereo). Replays the session frame
|
||||
// by frame, feeding the stored odom pose straight to Rtabmap (no odometry
|
||||
// recomputation). Sweeps the 2x2x2 grid:
|
||||
// Optimizer/Strategy in {g2o, GTSAM}
|
||||
// Optimizer/Robust in {true, false}
|
||||
// RGBD/OptimizeMaxError in {3.0, 0.0}
|
||||
// The full robust combo (g2o + Robust=true + MaxError=3) is the golden;
|
||||
// runs with both safeguards off should produce a visibly wrong graph.
|
||||
// ---------------------------------------------------------------------------
|
||||
TEST_F(RtabmapIntegrationFixture, RobustGraphOptimizationStereo)
|
||||
{
|
||||
const std::string srcPath = testDataPath("robust_graph_optimization_stereo.db");
|
||||
SKIP_IF_MISSING(srcPath);
|
||||
|
||||
struct Variant {
|
||||
Optimizer::Type optType;
|
||||
bool robust;
|
||||
float maxError;
|
||||
const char * label;
|
||||
};
|
||||
// Ceres only appears in Robust=false variants; Optimizer/Robust is
|
||||
// implemented via Vertigo switches that only g2o and GTSAM support.
|
||||
const std::vector<Variant> variants = {
|
||||
{Optimizer::kTypeG2O, true, 3.0f, "g2o-robust-maxerr3" },
|
||||
{Optimizer::kTypeG2O, true, 0.0f, "g2o-robust-maxerr0" },
|
||||
{Optimizer::kTypeG2O, false, 3.0f, "g2o-norobust-maxerr3" },
|
||||
{Optimizer::kTypeG2O, false, 0.0f, "g2o-norobust-maxerr0" },
|
||||
{Optimizer::kTypeGTSAM, true, 3.0f, "gtsam-robust-maxerr3" },
|
||||
{Optimizer::kTypeGTSAM, true, 0.0f, "gtsam-robust-maxerr0" },
|
||||
{Optimizer::kTypeGTSAM, false, 3.0f, "gtsam-norobust-maxerr3" },
|
||||
{Optimizer::kTypeGTSAM, false, 0.0f, "gtsam-norobust-maxerr0" },
|
||||
{Optimizer::kTypeCeres, false, 3.0f, "ceres-norobust-maxerr3" },
|
||||
{Optimizer::kTypeCeres, false, 0.0f, "ceres-norobust-maxerr0" },
|
||||
};
|
||||
|
||||
// Golden trajectory captured from the g2o-robust-maxerr3 variant and
|
||||
// committed under data/tests/. Regenerate by uncommenting the
|
||||
// exportPoses block below and rerunning the test.
|
||||
const std::string goldenPath = testDataPath("robust_graph_optimization_stereo_gt.g2o");
|
||||
std::map<int, Transform> goldenPoses;
|
||||
std::multimap<int, Link> goldenLinks;
|
||||
ASSERT_TRUE(graph::importPoses(
|
||||
goldenPath, /*format=*/4, goldenPoses, &goldenLinks))
|
||||
<< "Failed to load golden poses from " << goldenPath;
|
||||
|
||||
int variantsTested = 0;
|
||||
for(const Variant & v : variants)
|
||||
{
|
||||
if(!Optimizer::isAvailable(v.optType))
|
||||
{
|
||||
std::cerr << "[skip] " << v.label << " (optimizer unavailable)\n";
|
||||
continue;
|
||||
}
|
||||
SCOPED_TRACE(std::string(v.label));
|
||||
|
||||
ParametersMap params = baseRtabmapParams();
|
||||
uInsert(params, ParametersPair(Parameters::kOptimizerStrategy(),
|
||||
uNumber2Str(static_cast<int>(v.optType))));
|
||||
uInsert(params, ParametersPair(Parameters::kOptimizerRobust(),
|
||||
v.robust ? "true" : "false"));
|
||||
uInsert(params, ParametersPair(Parameters::kRGBDOptimizeMaxError(),
|
||||
uNumber2Str(v.maxError)));
|
||||
// Reuse the keypoints/descriptors that DBReader replays from the
|
||||
// source DB; skip Rtabmap's own feature extraction step.
|
||||
uInsert(params, ParametersPair(Parameters::kMemUseOdomFeatures(), "true"));
|
||||
|
||||
const std::string workDb = test::tempPath(uFormat(
|
||||
"rtabmap_integration_RobustGraphOptimizationStereo_%s.db", v.label));
|
||||
std::cerr << "Working DB for " << v.label << ": " << workDb << "\n";
|
||||
|
||||
const ReplayResult result =
|
||||
replayDatabaseWithStoredOdom(srcPath, workDb, params);
|
||||
|
||||
// Sanity: every variant must produce some optimized graph.
|
||||
ASSERT_GT(result.framesProcessed, 0) << v.label << " produced no frames";
|
||||
ASSERT_GT(result.finalGlobalGraphSize, 0)
|
||||
<< v.label << " produced empty graph";
|
||||
|
||||
// Regenerate golden from the most robust combo. Toggled on for a
|
||||
// one-shot capture; re-comment after the file is committed.
|
||||
// if(v.optType == Optimizer::kTypeG2O && v.robust && v.maxError > 0.0f)
|
||||
// {
|
||||
// std::multimap<int, Link> links;
|
||||
// rtabmap::graph::exportPoses(goldenPath, /*format=*/4,
|
||||
// result.finalGlobalPoses, links);
|
||||
// }
|
||||
|
||||
float tRmse=0, tMean=0, tMed=0, tStd=0, tMin=0, tMax=0;
|
||||
float rRmse=0, rMean=0, rMed=0, rStd=0, rMin=0, rMax=0;
|
||||
graph::calcRMSE(goldenPoses, result.finalGlobalPoses,
|
||||
tRmse, tMean, tMed, tStd, tMin, tMax,
|
||||
rRmse, rMean, rMed, rStd, rMin, rMax,
|
||||
/*align2D=*/false);
|
||||
std::cerr << "[" << v.label << "] vs golden (aligned): "
|
||||
<< "trans rmse=" << tRmse << "m max=" << tMax << "m, "
|
||||
<< "rot rmse=" << rRmse << "deg max=" << rMax << "deg\n";
|
||||
|
||||
// Either safeguard (robust optimizer or MaxError-gated link
|
||||
// rejection) is enough to keep the graph close to the golden
|
||||
// trajectory. With BOTH disabled, the bad loop closure(s) in
|
||||
// this dataset destroy the graph — translational drift jumps
|
||||
// to >1 m and rotation to >40°. The bounds below encode that
|
||||
// "either safeguard alone is enough; neither is catastrophic"
|
||||
// invariant.
|
||||
const bool isSuperWrong = !v.robust && v.maxError == 0.0f;
|
||||
if(isSuperWrong)
|
||||
{
|
||||
EXPECT_GT(tRmse, 0.5f)
|
||||
<< v.label << " expected to be visibly wrong "
|
||||
<< "(neither Robust nor MaxError enabled)";
|
||||
}
|
||||
else
|
||||
{
|
||||
EXPECT_LT(tRmse, 0.05f)
|
||||
<< v.label << " translational RMSE too large; "
|
||||
<< "expected near golden";
|
||||
EXPECT_LT(rRmse, 1.5f)
|
||||
<< v.label << " rotational RMSE too large; "
|
||||
<< "expected near golden";
|
||||
}
|
||||
++variantsTested;
|
||||
}
|
||||
ASSERT_GT(variantsTested, 0) << "no compatible optimizer was available";
|
||||
}
|
||||
|
||||
@@ -0,0 +1,112 @@
|
||||
PARAMS_SE3OFFSET 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
|
||||
VERTEX_SE3:QUAT 1 -0.000588 0.000697 -0.000381 -0.000138 -0.000704 -0.000098 1.000000
|
||||
VERTEX_SE3:QUAT 2 -0.001746 -0.001330 0.001238 -0.000815 -0.000794 0.000285 0.999999
|
||||
VERTEX_SE3:QUAT 3 0.006394 -0.017468 0.003849 0.060383 0.004121 0.020493 0.997956
|
||||
VERTEX_SE3:QUAT 4 -0.012999 -0.018077 0.017656 0.081910 0.014447 -0.013553 0.996443
|
||||
VERTEX_SE3:QUAT 5 -0.059870 -0.044833 0.094304 0.015522 0.031786 -0.052220 0.998009
|
||||
VERTEX_SE3:QUAT 6 -0.102590 -0.077858 0.209392 0.005493 0.102174 -0.088097 0.990843
|
||||
VERTEX_SE3:QUAT 7 -0.276664 -0.208604 0.325637 0.026082 0.223528 -0.074240 0.971516
|
||||
VERTEX_SE3:QUAT 8 -0.497747 -0.267956 0.378819 0.028091 0.243712 -0.134593 0.960052
|
||||
VERTEX_SE3:QUAT 9 -0.796271 -0.469040 0.383643 0.023578 0.254910 -0.067568 0.964313
|
||||
VERTEX_SE3:QUAT 10 -1.131408 -0.470347 0.415807 0.029082 0.244545 -0.038766 0.968426
|
||||
VERTEX_SE3:QUAT 11 -1.397659 -0.599801 0.406205 0.029847 0.245412 -0.044539 0.967935
|
||||
VERTEX_SE3:QUAT 12 -1.590605 -0.503684 0.411695 0.078647 0.240592 -0.214759 0.943297
|
||||
VERTEX_SE3:QUAT 13 -1.778995 -0.431561 0.423288 0.094560 0.250101 -0.329163 0.905626
|
||||
VERTEX_SE3:QUAT 14 -1.757597 -0.248909 0.435038 0.130259 0.207631 -0.528425 0.812828
|
||||
VERTEX_SE3:QUAT 15 -1.858994 -0.183908 0.469981 0.127240 0.185193 -0.649737 0.726192
|
||||
VERTEX_SE3:QUAT 16 -1.733861 -0.163761 0.440168 0.159369 0.160977 -0.796979 0.559921
|
||||
VERTEX_SE3:QUAT 17 -1.891272 -0.364012 0.454896 -0.162350 -0.113620 0.879101 -0.433491
|
||||
VERTEX_SE3:QUAT 18 -1.979482 -0.557643 0.460913 -0.170698 -0.050680 0.967381 -0.180191
|
||||
VERTEX_SE3:QUAT 19 -2.211049 -0.487745 0.482243 -0.172609 -0.013461 0.984017 -0.041656
|
||||
VERTEX_SE3:QUAT 20 -2.490026 -0.452545 0.488671 -0.178993 0.005302 0.980268 0.083714
|
||||
VERTEX_SE3:QUAT 21 -2.686179 -0.368891 0.468463 -0.192607 0.006921 0.973222 0.125279
|
||||
VERTEX_SE3:QUAT 22 -2.871901 -0.299972 0.431353 -0.172299 -0.004380 0.979885 0.100600
|
||||
VERTEX_SE3:QUAT 23 -2.986505 -0.299102 0.372929 -0.193429 -0.002372 0.973201 0.124337
|
||||
VERTEX_SE3:QUAT 24 -3.153621 -0.294500 0.273749 -0.188781 -0.002594 0.969955 0.153433
|
||||
VERTEX_SE3:QUAT 25 -3.220417 -0.256617 0.225279 -0.192474 0.000207 0.975451 0.107003
|
||||
VERTEX_SE3:QUAT 26 -3.232723 -0.223107 0.237870 -0.180352 0.019230 0.976353 0.117640
|
||||
VERTEX_SE3:QUAT 27 -3.261827 -0.213750 0.222916 -0.206233 0.011066 0.967964 0.142799
|
||||
VERTEX_SE3:QUAT 28 -3.321295 -0.236731 0.165676 -0.236797 -0.012122 0.961576 0.138392
|
||||
VERTEX_SE3:QUAT 29 -3.397747 -0.269026 0.127865 -0.250325 0.007776 0.956604 0.148946
|
||||
VERTEX_SE3:QUAT 30 -3.488882 -0.323022 0.097949 -0.248337 0.006171 0.959939 0.129645
|
||||
VERTEX_SE3:QUAT 31 -3.527627 -0.334520 0.104594 -0.210958 0.004771 0.968379 0.133098
|
||||
VERTEX_SE3:QUAT 32 -3.516028 -0.339240 0.136733 -0.163617 -0.013072 0.977263 0.134226
|
||||
VERTEX_SE3:QUAT 33 -3.464536 -0.349685 0.132876 -0.208674 -0.007141 0.969541 0.128043
|
||||
VERTEX_SE3:QUAT 34 -3.316840 -0.377701 0.174692 -0.227423 -0.010040 0.971399 0.067546
|
||||
VERTEX_SE3:QUAT 35 -3.147453 -0.332530 0.241275 -0.241574 -0.034635 0.969228 0.032253
|
||||
VERTEX_SE3:QUAT 36 -2.974620 -0.337413 0.358935 -0.241771 -0.015896 0.968859 0.051043
|
||||
VERTEX_SE3:QUAT 37 -2.751030 -0.343430 0.433965 -0.221328 -0.007341 0.973584 0.055618
|
||||
VERTEX_SE3:QUAT 38 -2.561446 -0.211606 0.427566 -0.245316 -0.019081 0.968366 0.041504
|
||||
VERTEX_SE3:QUAT 39 -2.530088 -0.123678 0.420029 -0.260495 0.000713 0.959652 0.105879
|
||||
VERTEX_SE3:QUAT 40 -2.581700 0.035284 0.421437 -0.279732 0.019540 0.934500 0.219265
|
||||
VERTEX_SE3:QUAT 41 -2.649338 0.199804 0.439701 -0.249010 0.068833 0.910622 0.322528
|
||||
VERTEX_SE3:QUAT 42 -2.525040 0.533190 0.455043 -0.230150 0.089711 0.891703 0.379274
|
||||
VERTEX_SE3:QUAT 43 -2.541450 0.700266 0.457174 -0.226993 0.143996 0.828455 0.491326
|
||||
VERTEX_SE3:QUAT 44 -2.482325 0.896737 0.453420 -0.216869 0.194985 0.750384 0.593189
|
||||
VERTEX_SE3:QUAT 45 -2.327927 1.190995 0.471401 -0.192991 0.211629 0.676004 0.678959
|
||||
VERTEX_SE3:QUAT 46 -2.260120 1.487004 0.487421 -0.143384 0.232822 0.588032 0.761219
|
||||
VERTEX_SE3:QUAT 47 -2.034385 1.793372 0.504576 -0.125581 0.211226 0.532897 0.809712
|
||||
VERTEX_SE3:QUAT 48 -1.890588 2.149989 0.509821 -0.095699 0.212677 0.457765 0.857940
|
||||
VERTEX_SE3:QUAT 49 -1.711467 2.515326 0.497812 -0.111257 0.208176 0.452463 0.859977
|
||||
VERTEX_SE3:QUAT 50 -1.555815 2.913306 0.503440 -0.091206 0.201721 0.443391 0.868559
|
||||
VERTEX_SE3:QUAT 51 -1.390602 3.342880 0.504608 -0.097386 0.189555 0.439844 0.872423
|
||||
VERTEX_SE3:QUAT 52 -1.221037 3.679859 0.493722 -0.119047 0.171606 0.521568 0.827252
|
||||
VERTEX_SE3:QUAT 53 -1.162915 4.135812 0.503778 -0.106447 0.175816 0.517354 0.830724
|
||||
VERTEX_SE3:QUAT 54 -1.072481 4.620525 0.500020 -0.123923 0.147032 0.590872 0.783514
|
||||
VERTEX_SE3:QUAT 55 -1.007350 5.175958 0.518097 -0.094426 0.159944 0.574089 0.797448
|
||||
VERTEX_SE3:QUAT 56 -0.912963 5.643619 0.520774 -0.115771 0.147789 0.583914 0.789810
|
||||
VERTEX_SE3:QUAT 57 -0.786779 6.020124 0.516491 -0.099375 0.184817 0.583512 0.784526
|
||||
VERTEX_SE3:QUAT 58 -0.711759 6.170470 0.521791 -0.090641 0.183248 0.542054 0.815096
|
||||
VERTEX_SE3:QUAT 59 -0.737182 6.274137 0.536293 -0.061849 0.195092 0.565066 0.799259
|
||||
VERTEX_SE3:QUAT 60 -0.685063 6.342995 0.528836 -0.059614 0.209560 0.549067 0.806881
|
||||
VERTEX_SE3:QUAT 61 -0.650197 6.420473 0.528174 -0.056655 0.210199 0.509236 0.832638
|
||||
VERTEX_SE3:QUAT 62 -0.548423 6.584886 0.544962 -0.084880 0.174805 0.634616 0.747998
|
||||
VERTEX_SE3:QUAT 63 -0.672002 6.780437 0.550250 -0.100117 0.138312 0.729807 0.661989
|
||||
VERTEX_SE3:QUAT 64 -0.707670 6.843091 0.519993 -0.137553 0.090644 0.852172 0.496654
|
||||
VERTEX_SE3:QUAT 65 -0.858619 6.765769 0.502166 -0.160978 0.040489 0.933274 0.318506
|
||||
VERTEX_SE3:QUAT 66 -1.072275 6.600192 0.501476 -0.171498 0.013205 0.958414 0.227720
|
||||
VERTEX_SE3:QUAT 67 -1.171380 6.491982 0.496936 -0.177180 -0.049331 0.981949 0.044158
|
||||
VERTEX_SE3:QUAT 68 -1.173780 6.157156 0.490889 -0.175998 -0.036016 0.983538 -0.019502
|
||||
VERTEX_SE3:QUAT 69 -1.269421 5.927799 0.478131 -0.171770 -0.068675 0.975132 -0.122052
|
||||
VERTEX_SE3:QUAT 70 -1.348364 5.685798 0.465531 -0.180245 -0.073246 0.974697 -0.110061
|
||||
VERTEX_SE3:QUAT 71 -1.471405 5.417794 0.474820 -0.175216 -0.055695 0.979035 -0.087680
|
||||
VERTEX_SE3:QUAT 72 -1.634832 5.230772 0.498084 -0.173854 -0.043688 0.983135 -0.036216
|
||||
VERTEX_SE3:QUAT 73 -1.781609 5.029132 0.463230 -0.183916 -0.006005 0.982055 0.041307
|
||||
VERTEX_SE3:QUAT 74 -2.007541 5.014679 0.447555 -0.180418 -0.003933 0.981993 0.055884
|
||||
VERTEX_SE3:QUAT 75 -2.218553 4.969353 0.367418 -0.204523 0.003474 0.970224 0.129704
|
||||
VERTEX_SE3:QUAT 76 -2.395988 5.023928 0.315197 -0.204670 0.010144 0.972814 0.107888
|
||||
VERTEX_SE3:QUAT 77 -2.544660 5.003880 0.241153 -0.212089 -0.006484 0.969129 0.125563
|
||||
VERTEX_SE3:QUAT 78 -2.497301 4.997963 0.271344 -0.195179 -0.020919 0.975601 0.098338
|
||||
VERTEX_SE3:QUAT 79 -2.310430 4.954827 0.364736 -0.208959 -0.031316 0.974805 0.071488
|
||||
VERTEX_SE3:QUAT 80 -2.118914 4.825150 0.470425 -0.187232 -0.015330 0.977809 0.092726
|
||||
VERTEX_SE3:QUAT 81 -1.837082 4.741815 0.513113 -0.175699 -0.044024 0.982094 -0.051799
|
||||
VERTEX_SE3:QUAT 82 -1.689971 4.455073 0.513114 -0.164319 -0.046302 0.984108 -0.048852
|
||||
VERTEX_SE3:QUAT 83 -1.689234 4.150646 0.532252 -0.164442 -0.038118 0.971928 -0.163895
|
||||
VERTEX_SE3:QUAT 84 -1.738039 3.837260 0.519465 -0.145239 -0.080175 0.956545 -0.239789
|
||||
VERTEX_SE3:QUAT 85 -1.751354 3.446056 0.506697 -0.157954 -0.080039 0.930461 -0.320758
|
||||
VERTEX_SE3:QUAT 86 -1.824569 3.096938 0.484666 -0.137346 -0.130509 0.882334 -0.430801
|
||||
VERTEX_SE3:QUAT 87 -1.839057 2.575662 0.487885 -0.125410 -0.112166 0.873377 -0.457059
|
||||
VERTEX_SE3:QUAT 88 -1.835381 2.040903 0.475665 -0.128770 -0.100882 0.891300 -0.422876
|
||||
VERTEX_SE3:QUAT 89 -2.008950 1.510215 0.480892 -0.107261 -0.110829 0.889588 -0.429937
|
||||
VERTEX_SE3:QUAT 90 -2.118680 0.964879 0.474105 -0.127786 -0.077459 0.924397 -0.350943
|
||||
VERTEX_SE3:QUAT 91 -2.261652 0.495327 0.474904 -0.135071 -0.075173 0.943518 -0.293051
|
||||
VERTEX_SE3:QUAT 92 -2.428267 0.115464 0.453427 -0.148863 -0.052195 0.963804 -0.214935
|
||||
VERTEX_SE3:QUAT 93 -2.606732 -0.158458 0.447908 -0.167882 -0.003968 0.984229 -0.055613
|
||||
VERTEX_SE3:QUAT 94 -2.889626 -0.228438 0.385008 -0.206546 0.003059 0.978397 0.008348
|
||||
VERTEX_SE3:QUAT 95 -3.107210 -0.304825 0.309688 -0.228414 0.001150 0.965574 0.124470
|
||||
VERTEX_SE3:QUAT 96 -3.084566 -0.338251 0.335442 -0.222777 -0.010664 0.971590 0.079186
|
||||
VERTEX_SE3:QUAT 97 -2.815763 -0.303225 0.422228 -0.177353 -0.065348 0.974230 -0.123088
|
||||
VERTEX_SE3:QUAT 98 -2.513932 -0.293659 0.429209 -0.166492 -0.094962 0.948123 -0.253625
|
||||
VERTEX_SE3:QUAT 99 -2.351506 -0.142600 0.468308 0.096071 0.142100 -0.825158 0.538231
|
||||
VERTEX_SE3:QUAT 100 -2.125179 -0.050388 0.468116 0.073283 0.145816 -0.729483 0.664246
|
||||
VERTEX_SE3:QUAT 101 -2.106658 0.100650 0.441218 0.062113 0.159246 -0.608769 0.774715
|
||||
VERTEX_SE3:QUAT 102 -2.039725 0.204884 0.397495 0.040679 0.188449 -0.419552 0.887022
|
||||
VERTEX_SE3:QUAT 103 -1.697671 0.085757 0.400328 0.048806 0.167947 -0.410599 0.894886
|
||||
VERTEX_SE3:QUAT 104 -1.406671 -0.162093 0.362862 0.015504 0.182870 -0.313544 0.931670
|
||||
VERTEX_SE3:QUAT 105 -1.038748 -0.263786 0.354396 0.016815 0.172276 -0.250372 0.952550
|
||||
VERTEX_SE3:QUAT 106 -0.677447 -0.208984 0.361050 -0.003454 0.175975 -0.094098 0.979881
|
||||
VERTEX_SE3:QUAT 107 -0.352361 -0.141721 0.320618 0.008697 0.146522 -0.015146 0.989053
|
||||
VERTEX_SE3:QUAT 108 -0.099811 -0.015548 0.131005 -0.007868 0.110302 -0.025362 0.993543
|
||||
VERTEX_SE3:QUAT 109 -0.021109 0.004729 0.009034 0.043833 0.094815 -0.030571 0.994059
|
||||
VERTEX_SE3:QUAT 110 -0.018548 0.012909 -0.002822 -0.000645 0.084309 -0.057166 0.994798
|
||||
VERTEX_SE3:QUAT 111 -0.015478 0.007387 -0.001459 -0.003715 -0.000777 -0.035324 0.999369
|
||||
Reference in New Issue
Block a user