132 const gtsam::Vector3& imuAngularVelocity) {
135 auto priorPoseNoise = gtsam::noiseModel::Diagonal::Sigmas(
140 auto priorVelocityNoise = gtsam::noiseModel::Isotropic::Sigma(3,
graphConfigPtr_->initialVelocityStdDev_);
141 auto priorBiasNoise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(6) <<
graphConfigPtr_->initialAccBiasStdDev_,
150 gtsam::NonlinearFactorGraph newRtGraphFactors, newBatchGraphFactors;
151 gtsam::Values newRtGraphValues, newBatchGraphValues;
152 std::shared_ptr<std::map<gtsam::Key, double>> priorKeyTimestampMapPtr = std::make_shared<std::map<gtsam::Key, double>>();
159 gtsam::Values valuesEstimate;
160 REGULAR_COUT <<
" Initial Pose of imu in world frame: " << T_W_I0 << std::endl;
162 REGULAR_COUT <<
" Initial velocity assumed to be: " << gtsam::Vector3(0, 0, 0) << std::endl;
163 valuesEstimate.insert(gtsam::symbol_shorthand::V(
propagatedStateKey_), gtsam::Vector3(0, 0, 0));
182 gtsam::Vector3(0, 0, 0),
185 gtsam::Vector3(0, 0, 0),
224 T_W_O_ = (T_W_I0.inverse() * T_O_I0).matrix();
230 std::shared_ptr<SafeNavStateWithCovarianceAndBias>& newOptimizedNavStatePtr,
231 const std::shared_ptr<ImuBuffer>& imuBufferPtr,
const double imuTimeK,
bool createNewStateFlag) {
243 imuBufferPtr->getLastTwoMeasurements(imuMeas);
254 imuBufferPtr->integrateNavStateFromTimestamp(imuMeas.begin()->first, imuMeas.rbegin()->first,
W_imuPropagatedState_,
258 imuBufferPtr->integrateNavStateFromTimestamp(imuMeas.begin()->first, imuMeas.rbegin()->first,
W_imuPropagatedState_,
265 const gtsam::Pose3 T_Ikm1_Ik = T_W_I_beforeInt.between(T_W_I_afterInt);
269 const Eigen::Matrix3d& R_W_I = T_W_I_afterInt.rotation().matrix();
270 Eigen::Matrix3d R_O_I = T_O_I.rotation().matrix();
271 R_O_I.row(2) = R_W_I.row(2);
272 R_O_I.row(1) = R_O_I.row(2).cross(R_O_I.row(0)).normalized();
273 R_O_I.row(0) = R_O_I.row(1).cross(R_O_I.row(2)).normalized();
274 T_O_I = gtsam::Pose3(gtsam::Rot3(R_O_I), T_O_I.translation());
282 newOptimizedNavStatePtr = std::make_shared<SafeNavStateWithCovarianceAndBias>(
optimizedGraphState_);
284 newOptimizedNavStatePtr =
nullptr;
293 returnPreIntegratedNavState.
update(
T_W_O_, Eigen::Isometry3d(T_O_Ik_nav.pose().matrix()), T_O_Ik_nav.bodyVelocity(),
294 optimizedGraphState_.imuBias().correctGyroscope(imuMeas.rbegin()->second.angularVelocity), imuTimeK,
300 createNewStateFlag =
false;
302 <<
" The current measurement would fall outside of the real-time smoother lag, hence skipping the creation of a new"
303 <<
"IMU measurement. Not creating new state." <<
COLOR_END << std::endl;
307 if (createNewStateFlag) {
318 gtsam::CombinedImuFactor imuFactor(gtsam::symbol_shorthand::X(oldKey), gtsam::symbol_shorthand::V(oldKey),
319 gtsam::symbol_shorthand::X(newKey), gtsam::symbol_shorthand::V(newKey),
324 gtsam::Values valuesEstimate;
425 const double timeKm1,
const double timeK,
const double rate,
426 const RobustNormEnum& robustNormEnum,
const double robustNormConstant) {
428 const double maxLidarTimestampDistance = (1.0 / rate) + (2.0 *
graphConfigPtr_->maxSearchDeviation_);
429 gtsam::Key closestKeyKm1, closestKeyK;
430 double keyTimeStampDistance{0.0};
432 if (!
findGraphKeys_(closestKeyKm1, closestKeyK, keyTimeStampDistance, maxLidarTimestampDistance, timeKm1, timeK,
"pose between")) {
434 << closestKeyKm1 <<
" and " << closestKeyK <<
COLOR_END << std::endl;
439 const double scale = keyTimeStampDistance / (timeK - timeKm1);
441 REGULAR_COUT <<
" Scaling factor in tangent space for pose between delta pose: " << scale << std::endl;
443 gtsam::Pose3 scaledDeltaPose = gtsam::Pose3::Expmap(scale * gtsam::Pose3::Logmap(deltaPose));
446 assert(poseBetweenNoiseDensity.size() == 6);
447 auto noise = gtsam::noiseModel::Diagonal::Sigmas((gtsam::Vector(poseBetweenNoiseDensity)));
448 gtsam::noiseModel::Robust::shared_ptr robustErrorFunction;
450 switch (robustNormEnum) {
452 robustErrorFunction = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Huber::Create(robustNormConstant), noise);
455 robustErrorFunction = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Cauchy::Create(robustNormConstant), noise);
458 robustErrorFunction = gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::Tukey::Create(robustNormConstant), noise);
461 robustErrorFunction =
462 gtsam::noiseModel::Robust::Create(gtsam::noiseModel::mEstimator::GemanMcClure::Create(robustNormConstant), noise);
467 gtsam::BetweenFactor<gtsam::Pose3> poseBetweenFactor;
469 poseBetweenFactor = gtsam::BetweenFactor<gtsam::Pose3>(gtsam::symbol_shorthand::X(closestKeyKm1),
470 gtsam::symbol_shorthand::X(closestKeyK), scaledDeltaPose, noise);
472 poseBetweenFactor = gtsam::BetweenFactor<gtsam::Pose3>(gtsam::symbol_shorthand::X(closestKeyKm1),
473 gtsam::symbol_shorthand::X(closestKeyK), scaledDeltaPose, robustErrorFunction);
482 << closestKeyKm1 <<
" and key " << closestKeyK <<
COLOR_END << std::endl;
548 gtsam::NonlinearFactorGraph newRtGraphFactors, newBatchGraphFactors;
549 gtsam::Values newRtGraphValues, newBatchGraphValues;
550 std::map<gtsam::Key, double> newRtGraphKeysTimestampsMap, newBatchGraphKeysTimestampsMap;
551 gtsam::Key currentPropagatedKey;
552 gtsam::Vector3 currentGraphStateKeyAngularVelocity;
553 double currentGraphStateKeyTime;
589 bool successfulOptimizationFlag =
true;
591 if (newRtGraphFactors.size() > 0) {
595 REGULAR_COUT <<
" No factors present in rt smoother, not optimizing." << std::endl;
598 if (
graphConfigPtr_->useAdditionalSlowBatchSmootherFlag_ && newBatchGraphFactors.size() > 0) {
599 successfulOptimizationFlag =
602 REGULAR_COUT <<
" No factors present in batch smoother, not optimizing." << std::endl;
614 if (!successfulOptimizationFlag) {
621 gtsam::NavState resultNavState =
625 gtsam::imuBias::ConstantBias resultBias =
rtOptimizerPtr_->calculateEstimatedBias(gtsam::symbol_shorthand::B(currentPropagatedKey));
630 gtsam::Matrix66 resultPoseCovarianceWorldFrame =
633 gtsam::Matrix33 resultVelocityCovariance =
634 rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsam::symbol_shorthand::V(currentPropagatedKey));
651 const std::pair<std::string, std::string>& framePair = framePairKeyMapIterator.first;
654 const gtsam::Key& gtsamKey = framePairKeyMapIterator.second.key();
657 bool isLandmarkState =
false;
659 isLandmarkState =
true;
663 const char stateCategory = gtsam::Symbol(gtsamKey).chr();
666 if (framePairKeyMapIterator.second.isVariableActive()) {
669 gtsam::Pose3 T_frame1_frame2;
670 gtsam::Matrix66 T_frame1_frame2_covariance = gtsam::Z_6x6;
675 T_frame1_frame2_covariance =
rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsamKey);
677 framePairKeyMapIterator.second.updateLatestEstimate(T_frame1_frame2, T_frame1_frame2_covariance);
680 gtsam::Values valuesEstimate;
681 valuesEstimate.insert(gtsamKey, T_frame1_frame2);
687 gtsam::Point3 point3Estimate =
rtOptimizerPtr_->calculateEstimatedPoint3(gtsamKey);
688 T_frame1_frame2 = gtsam::Pose3(gtsam::Rot3::Identity(), point3Estimate);
689 T_frame1_frame2_covariance.block<3, 3>(3, 3) =
rtOptimizerPtr_->calculateMarginalCovarianceMatrixAtKey(gtsamKey);
691 framePairKeyMapIterator.second.updateLatestEstimate(T_frame1_frame2, T_frame1_frame2_covariance);
694 gtsam::Values valuesEstimate;
695 valuesEstimate.insert(gtsamKey, point3Estimate);
701 throw std::runtime_error(
"Key is neither a pose nor a displacement key.");
706 gtsam::Pose3 T_frame2_frame1_corrected = T_frame1_frame2.inverse();
707 T_frame2_frame1_corrected =
708 gtsam::Pose3(T_frame2_frame1_corrected.rotation(),
709 T_frame2_frame1_corrected.translation() + framePairKeyMapIterator.second.getReferenceFrameKeyframePosition());
710 gtsam::Pose3 T_frame1_frame2_corrected = T_frame2_frame1_corrected.inverse();
713 if (isLandmarkState) {
715 resultLandmarkTransformations.
set_T_frame1_frame2(framePairKeyMapIterator.first.first, framePairKeyMapIterator.first.second,
716 Eigen::Isometry3d(T_frame1_frame2_corrected.matrix()));
717 resultLandmarkTransformationsCovariance.
set_T_frame1_frame2(framePairKeyMapIterator.first.first,
718 framePairKeyMapIterator.first.second, T_frame1_frame2_covariance);
723 resultReferenceFrameTransformations.
set_T_frame1_frame2(framePairKeyMapIterator.first.first,
724 framePairKeyMapIterator.first.second,
725 Eigen::Isometry3d(T_frame1_frame2_corrected.matrix()));
727 framePairKeyMapIterator.first.first, framePairKeyMapIterator.first.second, T_frame1_frame2_covariance);
732 REGULAR_COUT <<
" Added frame pair: " << framePair.first <<
" to " << framePair.second <<
" to real-time logging."
739 if (framePairKeyMapIterator.second.getNumberStepsOptimized() == 0) {
741 REGULAR_COUT <<
" Fixed-frame Transformation between " << framePairKeyMapIterator.first.first <<
" and "
742 << framePairKeyMapIterator.first.second <<
" optimized for the first time." <<
COLOR_END << std::endl;
745 REGULAR_COUT <<
GREEN_START <<
" Result, RPY (deg): " << T_frame1_frame2.rotation().rpy().transpose() * (180.0 / M_PI)
746 <<
", t (x, y, z): " << T_frame1_frame2.translation().transpose() <<
COLOR_END << std::endl;
750 framePairKeyMapIterator.second.incrementNumberStepsOptimized();
753 if (framePairKeyMapIterator.first.second ==
worldFrame_ &&
755 const gtsam::Pose3& T_frame1_frame2_initial =
756 framePairKeyMapIterator.second.getApproximateTransformationBeforeOptimization();
757 const double errorTangentSpace = gtsam::Pose3::Logmap(T_frame1_frame2_initial.between(T_frame1_frame2)).norm();
760 if (errorTangentSpace >
graphConfigPtr_->referenceFramePosesResetThreshold_) {
763 << framePairKeyMapIterator.first.first <<
" and " << framePairKeyMapIterator.first.second
764 <<
" diverged too much. Removing from optimization and adding again freshly at next possibility." <<
COLOR_END
768 framePairKeyMapIterator.first.second);
774 catch (
const std::out_of_range& exception) {
776 if (framePairKeyMapIterator.second.getNumberStepsOptimized() > 0) {
777 REGULAR_COUT <<
RED_START <<
" OutOfRange-Exception while querying the active transformation and/or covariance at key "
778 << gtsam::Symbol(gtsamKey) <<
", for frame pair " << framePairKeyMapIterator.first.first <<
","
779 << framePairKeyMapIterator.first.second << std::endl
780 <<
" This happens if the requested variable is outside of the smoother window (e.g. because corresponding "
781 "measurement stopped). Hence, we keep this estimate unchanged and remove/deactivate it from the state "
786 framePairKeyMapIterator.first.second);
792 double variableAge = framePairKeyMapIterator.second.computeVariableAge(currentGraphStateKeyTime);
795 << framePairKeyMapIterator.first.first <<
" and " << framePairKeyMapIterator.first.second <<
" is too old ("
797 <<
")s and was never optimized. Removing from optimization and adding again freshly at next "
802 framePairKeyMapIterator.first.second);
807 <<
YELLOW_START <<
" Tried to query the transformation and/or covariance for frame pair "
808 << framePairKeyMapIterator.first.first <<
" to " << framePairKeyMapIterator.first.second
809 <<
", at key: " << gtsam::Symbol(gtsamKey)
810 <<
". Not yet available, as it was not yet optimized. Waiting for next optimization iteration until publishing it. "
811 "Current state key: "
812 << currentPropagatedKey <<
COLOR_END << std::endl;
828 optimizedGraphState_.updateNavStateAndBias(currentPropagatedKey, currentGraphStateKeyTime, resultNavState,
829 resultBias.correctGyroscope(currentGraphStateKeyAngularVelocity), resultBias);
831 optimizedGraphState_.updateReferenceFrameTransformsCovariance(resultReferenceFrameTransformationsCovariance);
833 optimizedGraphState_.updateLandmarkTransformsCovariance(resultLandmarkTransformationsCovariance);
834 optimizedGraphState_.updateCovariances(resultPoseCovarianceWorldFrame, resultVelocityCovariance);
940 std::map<std::string, std::ofstream> fileStreams;
943 if (!std::filesystem::exists(savePath + timeString)) {
944 std::filesystem::create_directories(savePath + timeString);
948 std::string stateCategoryString =
"rt_R_6D_transform";
953 std::string frameInformation =
"_" + framePair.first +
"_to_" + framePair.second;
955 std::replace(frameInformation.begin(), frameInformation.end(),
'/',
'_');
965 const double& timeStamp = mapPair.first;
968 for (
const auto& framePairTransform : referenceFrameTransforms.
getTransformsMap()) {
970 const std::pair<std::string, std::string>& framePair = framePairTransform.first;
972 const Eigen::Isometry3d& transform = framePairTransform.second;
974 std::string frameInformation =
"_" + framePair.first +
"_to_" + framePair.second;
976 std::replace(frameInformation.begin(), frameInformation.end(),
'/',
'_');
1036 const std::string& savePath,
const bool saveCovarianceFlag) {
1044 std::map<std::string, std::ofstream> fileStreams;
1047 std::time_t now_time_t = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now());
1048 std::tm now_tm = *std::localtime(&now_time_t);
1050 std::ostringstream oss;
1051 oss << std::put_time(&now_tm,
"%Y_%m_%d_%H_%M_%S");
1053 std::string timeString = oss.str();
1055 timeString.erase(std::remove(timeString.begin(), timeString.end(),
'\n'), timeString.end());
1057 std::replace(timeString.begin(), timeString.end(),
' ',
'_');
1060 if (!std::filesystem::exists(savePath + timeString)) {
1061 std::filesystem::create_directories(savePath + timeString);
1079 for (
const auto& keyPosePair : optimizedValues.extract<gtsam::Pose3>()) {
1081 const gtsam::Key& graphKey = keyPosePair.first;
1082 gtsam::Pose3 pose = keyPosePair.second;
1083 const gtsam::Symbol stateSymbol(graphKey);
1084 const char stateCategory = stateSymbol.chr();
1085 const double timeStamp = keyTimestampMap.at(graphKey);
1088 std::string stateCategoryString =
"";
1089 std::string frameInformation =
"";
1092 const std::string stateCategoryCapital(1,
static_cast<char>(std::toupper(stateCategory)));
1096 if (stateCategory ==
'x') {
1097 stateCategoryString = stateCategoryCapital +
"_state_6D_pose";
1102 stateCategoryString = stateCategoryCapital +
"_6D_transform";
1104 std::pair<std::string, std::string> framePair;
1106 frameInformation =
"_" + framePair.first +
"_to_" + framePair.second;
1109 throw std::runtime_error(
"Could not find frame pair for key.");
1114 throw std::logic_error(stateCategoryString +
" is an unknown 6D state category. Check the admissible GTSAM symbols in the config.");
1117 std::string transformIdentifier = stateCategoryString + frameInformation;
1119 std::replace(transformIdentifier.begin(), transformIdentifier.end(),
'/',
'_');
1122 Eigen::Vector3d keyframePosition = Eigen::Vector3d::Zero();
1128 fileLogger_.createPose3CsvFileStream(fileStreams, savePath, transformIdentifier, timeString, saveCovarianceFlag);
1130 fileLogger_.createPose3TumFileStream(fileStreams, savePath, transformIdentifier +
"_tum", timeString);
1133 Eigen::Matrix<double, 6, 6> poseCovarianceInWorldRos = Eigen::Matrix<double, 6, 6>::Zero();
1134 if (saveCovarianceFlag) {
1141 if (keyframePosition.norm() > 1e-6) {
1142 const gtsam::Pose3& T_W_M = pose;
1143 gtsam::Pose3 T_M_W_corrected = T_W_M.inverse();
1144 T_M_W_corrected = gtsam::Pose3(T_M_W_corrected.rotation(),
1145 T_M_W_corrected.translation() + keyframePosition);
1146 gtsam::Pose3 T_W_M_corrected = T_M_W_corrected.inverse();
1147 pose = T_W_M_corrected;
1148 REGULAR_COUT <<
" Corrected frame position for " << transformIdentifier <<
" (" << gtsam::Symbol(graphKey) <<
") by "
1149 << keyframePosition.transpose() << std::endl;
1154 fileLogger_.writePose3ToCsvFile(fileStreams, pose, transformIdentifier, timeStamp, saveCovarianceFlag, poseCovarianceInWorldRos);
1156 fileLogger_.writePose3ToTumFile(fileStreams, pose, transformIdentifier +
"_tum", timeStamp);
1160 for (
const auto& keyVectorPair : optimizedValues.extract<gtsam::Point3>()) {
1162 const gtsam::Key& key = keyVectorPair.first;
1163 const gtsam::Vector& vector = keyVectorPair.second;
1164 const gtsam::Symbol symbol(key);
1165 const char stateCategory = symbol.chr();
1166 const double timeStamp = keyTimestampMap.at(key);
1169 std::string stateCategoryString =
"";
1170 std::string frameInformation =
"";
1173 const std::string stateCategoryNonCapital(1, stateCategory);
1177 if (stateCategory ==
'v') {
1178 stateCategoryString = stateCategoryNonCapital +
"_state_3D_velocity";
1183 stateCategoryString = stateCategoryNonCapital +
"_3D_vector";
1185 if (stateCategory ==
'l') {
1190 std::pair<std::string, std::string> framePair;
1192 frameInformation =
"_" + framePair.first +
"_to_" + framePair.second;
1199 throw std::logic_error(stateCategoryString +
" is an unknown 3D state category. Check the admissible GTSAM symbols in the config.");
1202 std::string transformIdentifier = stateCategoryString + frameInformation;
1206 fileLogger_.createPoint3CsvFileStream(fileStreams, savePath, transformIdentifier, timeString);
1209 fileLogger_.writePoint3ToCsvFile(fileStreams, vector, transformIdentifier, timeStamp);
1213 for (
const auto& keyBiasPair : optimizedValues.extract<gtsam::imuBias::ConstantBias>()) {
1215 const gtsam::Key& key = keyBiasPair.first;
1216 const gtsam::imuBias::ConstantBias& bias = keyBiasPair.second;
1217 const gtsam::Symbol symbol(key);
1218 const char stateCategory = symbol.chr();
1219 const double timeStamp = keyTimestampMap.at(key);
1222 assert(stateCategory ==
'b');
1225 const std::string stateCategoryCapital(1,
static_cast<char>(std::toupper(stateCategory)));
1229 std::string stateCategoryIdentifier = stateCategoryCapital +
"_imu_bias";
1233 fileLogger_.createImuBiasCsvFileStream(fileStreams, savePath, stateCategoryIdentifier, timeString);
1236 fileLogger_.writeImuBiasToCsvFile(fileStreams, bias, stateCategoryIdentifier, timeStamp);
1240 for (
auto& pair : fileStreams) {
1241 pair.second.close();