Skip to content

Commit 2a2605d

Browse files
nubertjnubertj
committed
update intialization for offline grah
1 parent 3be3316 commit 2a2605d

File tree

8 files changed

+69
-14
lines changed

8 files changed

+69
-14
lines changed

Diff for: ‎examples/atn_position3_fuser/config/core/core_graph_params.yaml

+10-10
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
# Sensor Params
22
sensor_params:
33
imuRate: 100.0 # Rate of IMU input (Hz)
4-
createStateEveryNthImuMeasurement: 2 # Create a new state every n-th IMU measurement
4+
createStateEveryNthImuMeasurement: 1 # Create a new state every n-th IMU measurement
55
useImuSignalLowPassFilter: false # If true, a low pass filter is applied to the IMU measurements
66
imuLowPassFilterCutoffFreq: 30.0 # Cutoff frequency of the low pass filter
77
imuBufferLength: 400
@@ -65,15 +65,15 @@ noise_params:
6565

6666
# Relinearization
6767
relinearization_params:
68-
positionReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Position linearization threshold
69-
rotationReLinTh: 1.0e-03 # Letter "x" in GTSAM variables, Rotation linearization threshold
70-
velocityReLinTh: 1.0e-03 # Letter "v" in GTSAM variables, Linear Velocity linearization threshold
71-
accBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Accelerometer bias linearization threshold
72-
gyrBiasReLinTh: 1.0e-03 # Letter "b" in GTSAM variables, Gyroscope bias linearization threshold
73-
referenceFrameReLinTh: 1.0e-03 # Letter "r" in GTSAM variables, Reference frame linearization threshold, ONLY IF optimizeReferenceFramePosesWrtWorld
74-
calibrationReLinTh: 1.0e-03 # Letter "c" in GTSAM variables, Calibration linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset
75-
displacementReLinTh: 1.0e-03 # Letter "d" in GTSAM variables, Displacement linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset
76-
landmarkReLinTh: 1.0e-03 # Letter "l" in GTSAM variables, Landmark linearization threshold
68+
positionReLinTh: 1.0e-02 # Letter "x" in GTSAM variables, Position linearization threshold
69+
rotationReLinTh: 1.0e-02 # Letter "x" in GTSAM variables, Rotation linearization threshold
70+
velocityReLinTh: 1.0e-02 # Letter "v" in GTSAM variables, Linear Velocity linearization threshold
71+
accBiasReLinTh: 1.0e-02 # Letter "b" in GTSAM variables, Accelerometer bias linearization threshold
72+
gyrBiasReLinTh: 1.0e-02 # Letter "b" in GTSAM variables, Gyroscope bias linearization threshold
73+
referenceFrameReLinTh: 1.0e-02 # Letter "r" in GTSAM variables, Reference frame linearization threshold, ONLY IF optimizeReferenceFramePosesWrtWorld
74+
calibrationReLinTh: 1.0e-02 # Letter "c" in GTSAM variables, Calibration linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset
75+
displacementReLinTh: 1.0e-02 # Letter "d" in GTSAM variables, Displacement linearization threshold, ONLY IF optimizeExtrinsicSensorToSensorCorrectedOffset
76+
landmarkReLinTh: 1.0e-02 # Letter "l" in GTSAM variables, Landmark linearization threshold
7777
relinearizeSkip: 1 # Re-linearization is skipped every n-th step, default: 10
7878
enableRelinearization: true # Whether to use relinearization, default: true
7979
evaluateNonlinearError: true # Whether to evaluate the nonlinear errorr before and after the update, default: false

Diff for: ‎examples/atn_position3_fuser/config/position_graph_specific/position_graph_graph_params.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -13,6 +13,6 @@ alignment_params:
1313
# Noise Parameters
1414
noise_params:
1515
## Position measurement
16-
prismPositionMeasUnaryNoiseDensity: 0.04 # x, y, z of global position
16+
prismPositionMeasUnaryNoiseDensity: 0.002 # x, y, z of global position
1717
gnssPositionMeasUnaryNoiseDensity: 0.04 # x, y, z of global position
1818
gnssOfflinePoseMeasUnaryNoiseDensity: [ 0.05, 0.05, 0.05, 0.2, 0.2, 0.2 ] # StdDev, ORDER RPY(rad) - XYZ(meters)

Diff for: ‎graph_msf/include/graph_msf/core/optimizer/OptimizerBase.h

+1
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,7 @@ class OptimizerBase {
2929
virtual bool update() = 0;
3030
virtual bool update(const gtsam::NonlinearFactorGraph& newGraphFactors, const gtsam::Values& newGraphValues,
3131
const std::map<gtsam::Key, double>& newGraphKeysTimeStampMap) = 0;
32+
virtual bool updateExistingValues(const gtsam::Values& newGraphValues) = 0;
3233
// Run (Batch) Optimization and get result
3334
virtual void optimize(int maxIterations) = 0;
3435
virtual const gtsam::Values& getAllOptimizedStates() = 0;

Diff for: ‎graph_msf/include/graph_msf/core/optimizer/OptimizerIsam2.hpp

+6
Original file line numberDiff line numberDiff line change
@@ -88,6 +88,12 @@ class OptimizerIsam2 : public OptimizerBase {
8888
}
8989
~OptimizerIsam2() = default;
9090

91+
// Implementation
92+
bool updateExistingValues(const gtsam::Values& newGraphValues) override {
93+
// Update Values
94+
throw std::logic_error("GraphMSF: OptimizerIsam2: updateExistingValues: Not implemented.");
95+
}
96+
9197
protected:
9298
// Parameters
9399
gtsam::ISAM2Params isam2Params_;

Diff for: ‎graph_msf/include/graph_msf/core/optimizer/OptimizerLM.hpp

+12
Original file line numberDiff line numberDiff line change
@@ -22,9 +22,21 @@ class OptimizerLM : public OptimizerBase {
2222
// Standard LM Parameters
2323
lmParams_.setVerbosity("ERROR");
2424
gtsam::LevenbergMarquardtParams::SetCeresDefaults(&lmParams_);
25+
26+
// Set Custom LM Parameters
27+
// lmParams_.setLinearSolverType(
28+
// "CHOLMOD"); // "MULTIFRONTAL_CHOLESKY", "MULTIFRONTAL_QR", "SEQUENTIAL_CHOLESKY", "SEQUENTIAL_QR", "ITERATIVE", "CHOLMOD"
29+
lmParams_.setRelativeErrorTol(1e-8);
30+
// lmParams_.minModelFidelity = 1e-1;
2531
}
2632
~OptimizerLM() = default;
2733

34+
// Implementation
35+
bool updateExistingValues(const gtsam::Values& newGraphValues) override {
36+
// Update Values
37+
throw std::logic_error("GraphMSF: OptimizerLM: updateExistingValues: Not implemented.");
38+
}
39+
2840
protected:
2941
// Parameters
3042
gtsam::LevenbergMarquardtParams lmParams_;

Diff for: ‎graph_msf/include/graph_msf/core/optimizer/OptimizerLMBatch.hpp

+20-1
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,25 @@ class OptimizerLMBatch : public OptimizerLM {
5151
return true;
5252
}
5353

54+
// Implementation
55+
bool updateExistingValues(const gtsam::Values& newGraphValues) override {
56+
// Update Values
57+
for (const auto& key : newGraphValues.keys()) {
58+
// Check whether key is in the batchSmootherValues
59+
if (containerBatchSmootherValues_.exists(key)) {
60+
// Remove old value
61+
containerBatchSmootherValues_.erase(key);
62+
// Insert new value
63+
containerBatchSmootherValues_.insert(key, newGraphValues.at(key));
64+
// std::cout << "GraphMSF: OptimizerLMBatch: updateExistingValues: Key " << gtsam::Symbol(key) << " updated." << std::endl;
65+
} else {
66+
throw std::logic_error("GraphMSF: OptimizerLMBatch: updateExistingValues: Key " + gtsam::Symbol(key).string() +
67+
" does not exist in batchSmootherValues.");
68+
}
69+
}
70+
return true;
71+
}
72+
5473
// Optimize
5574
void optimize(int maxIterations) override {
5675
// Print
@@ -310,7 +329,7 @@ class OptimizerLMBatch : public OptimizerLM {
310329
continue; // Skip timestamp of first reference frame key
311330
} else if (numReferenceFrameKeysInFactor > 1) {
312331
std::cout << "Found random walk factor with (at least) two reference frame keys (including " << gtsam::Symbol(factorKey)
313-
<< ")." << std::endl; // Do not skip
332+
<< ")." << std::endl; // Do not skip
314333
}
315334
}
316335

Diff for: ‎graph_msf/include/graph_msf/factors/gmsf_expression/GmsfUnaryExpressionAbsolut.h

+5-1
Original file line numberDiff line numberDiff line change
@@ -98,9 +98,13 @@ class GmsfUnaryExpressionAbsolut : public GmsfUnaryExpression<GTSAM_MEASUREMENT_
9898
gtsamDynamicExpressionKeys.get<gtsam::Pose3>()
9999
.lv_T_frame1_frame2(gmsfUnaryAbsoluteMeasurementPtr_->worldFrameName(), gmsfUnaryAbsoluteMeasurementPtr_->fixedFrameName())
100100
.activateVariable();
101+
// Set number of steps optimized to zero
102+
gtsamDynamicExpressionKeys.get<gtsam::Pose3>()
103+
.lv_T_frame1_frame2(gmsfUnaryAbsoluteMeasurementPtr_->worldFrameName(), gmsfUnaryAbsoluteMeasurementPtr_->fixedFrameName())
104+
.resetNumberStepsOptimized();
101105
// Print out
102106
REGULAR_COUT << GREEN_START << " Reactivated old keyframe " << gtsam::Symbol(graphKey.key())
103-
<< " as it was deactivated, but is needed again." << COLOR_END << std::endl;
107+
<< " as it was deactivated, but is needed again. Also set number of optimization steps to 0." << COLOR_END << std::endl;
104108

105109
// If the old keyframe was not active, then it is not part of the optimization anymore (or has never been) --> prior to online graph
106110
// Make sure that old key ptr is not null

Diff for: ‎graph_msf/src/lib/GraphManager.cpp

+14-1
Original file line numberDiff line numberDiff line change
@@ -572,13 +572,26 @@ void GraphManager::updateGraph() {
572572
T_frame1_frame2_covariance = rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsamKey);
573573
// Add current belief back to the map
574574
framePairKeyMapIterator.second.updateLatestEstimate(T_frame1_frame2, T_frame1_frame2_covariance);
575+
// Update the initial guess for the offline graph
576+
if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
577+
gtsam::Values valuesEstimate;
578+
valuesEstimate.insert(gtsamKey, T_frame1_frame2);
579+
std::ignore = batchOptimizerPtr_->updateExistingValues(valuesEstimate);
580+
}
575581
}
576582
// 3D Position3 Vectors
577583
else if (isCharInCharArray<numDynamic3DStates>(stateCategory, dim3StateSymbols)) {
578-
T_frame1_frame2 = gtsam::Pose3(gtsam::Rot3::Identity(), rtOptimizerPtr_->calculateEstimatedPoint3(gtsamKey));
584+
gtsam::Point3 point3Estimate = rtOptimizerPtr_->calculateEstimatedPoint3(gtsamKey);
585+
T_frame1_frame2 = gtsam::Pose3(gtsam::Rot3::Identity(), point3Estimate);
579586
T_frame1_frame2_covariance.block<3, 3>(3, 3) = rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsamKey);
580587
// Add current belief back to the map
581588
framePairKeyMapIterator.second.updateLatestEstimate(T_frame1_frame2, T_frame1_frame2_covariance);
589+
// Update the initial guess for the offline graph
590+
if (graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_) {
591+
gtsam::Values valuesEstimate;
592+
valuesEstimate.insert(gtsamKey, point3Estimate);
593+
std::ignore = batchOptimizerPtr_->updateExistingValues(valuesEstimate);
594+
}
582595
}
583596
// Only these two types are allowed for dynamic states for now
584597
else {

0 commit comments

Comments
 (0)