37void HolisticFusion::setup(
const std::shared_ptr<GraphConfig> graphConfigPtr,
const std::shared_ptr<StaticTransforms> staticTransformsPtr) {
41 if (graphConfigPtr ==
nullptr || staticTransformsPtr ==
nullptr) {
42 throw std::runtime_error(
"setup() of inheriting classes did not has not been called before.");
50 throw std::logic_error(
"HolisticFusion: initialization_params.static_at_startup=false: in-motion initialization is not implemented.");
54 throw std::invalid_argument(
"HolisticFusion: additionalOptimizationIterations must be nonnegative.");
57 throw std::invalid_argument(
"HolisticFusion: createStateEveryNthImuMeasurement must be positive.");
65 std::cout <<
"HolisticFusion: Creating GraphManager." << std::endl;
68 std::cout <<
"HolisticFusion: GraphManager created." << std::endl;
72 REGULAR_COUT <<
" Initialized thread for optimizing the graph in parallel." << std::endl;
184 const std::string& sensorFrame) {
191 <<
" Initial pose of imu frame in world frame has been set to RPY (deg): " << R_W_I.rpy().transpose() * (180.0 / M_PI)
192 <<
", t (m): " << W_t_W_I.transpose() <<
"." <<
COLOR_END << std::endl;
205 const Eigen::Vector3d& linearAcc,
const Eigen::Vector3d& angularVel,
double imuTimeK,
206 std::shared_ptr<SafeIntegratedNavState>& returnPreIntegratedNavStatePtr,
207 std::shared_ptr<SafeNavStateWithCovarianceAndBias>& returnOptimizedStateWithCovarianceAndBiasPtr,
208 Eigen::Matrix<double, 6, 1>& returnAddedImuMeasurements) {
229 Eigen::Vector3d linearAccInMps2 = linearAcc;
231 constexpr double gravityConst = 9.80665;
232 linearAccInMps2 *= gravityConst;
236 const double accNorm = linearAccInMps2.norm();
240 }
else if (accNorm > 90) {
259 returnAddedImuMeasurements =
coreImuBufferPtr_->addToImuBuffer(imuTimeK, linearAccInMps2, angularVel);
267 double imuAttitudeRoll, imuAttitudePitch = 0.0;
268 if (!
alignImu_(imuAttitudeRoll, imuAttitudePitch)) {
271 REGULAR_COUT <<
" NOT ENOUGH IMU MESSAGES TO INITIALIZE POSE. WAITING FOR MORE..." << std::endl;
275 Eigen::Matrix3d R_W_I0_attitude = gtsam::Rot3::Ypr(0.0, imuAttitudePitch, imuAttitudeRoll).matrix();
276 gtsam::Rot3 R_W_Init0_attitude(
283 R_W_Init0_attitude = gtsam::Rot3::Ypr(0.0, R_W_Init0_attitude.pitch(), R_W_Init0_attitude.roll());
285 R_W_Init0_attitude.matrix() *
289 Eigen::Isometry3d T_O_Ik_attitude = Eigen::Isometry3d::Identity();
290 T_O_Ik_attitude.matrix().block<3, 3>(0, 0) = R_W_I0_attitude;
291 Eigen::Vector3d O_t_O_Ik =
292 Eigen::Vector3d(0, 0, 0) -
296 T_O_Ik_attitude.matrix().block<3, 1>(0, 3) = O_t_O_Ik;
298 <<
", hence iniital position of IMU is: " << O_t_O_Ik.transpose() << std::endl;
299 Eigen::Vector3d zeroPVeloctiy = Eigen::Vector3d(0, 0, 0);
300 preIntegratedNavStatePtr_ = std::make_shared<SafeIntegratedNavState>(T_O_Ik_attitude, zeroPVeloctiy, zeroPVeloctiy, imuTimeK);
310 REGULAR_COUT <<
" IMU callback waiting for initialization of global yaw and initial position." << std::endl;
326 initGraph_(imuTimeK, returnAddedImuMeasurements.tail<3>());
337 imuTimeK, createNewStateFlag);
351 static_cast<void>(
graphMgrPtr_->addPoseBetweenFactor(gtsam::Pose3::Identity(), noiseDensity * Eigen::Matrix<double, 6, 1>::Ones(),
353 graphMgrPtr_->addUnaryClassicFactor<gtsam::Vector3, 3, gtsam::PriorFactor<gtsam::Vector3>, gtsam::symbol_shorthand::V>(
354 gtsam::Vector3::Zero(), noiseDensity * Eigen::Matrix<double, 3, 1>::Ones(), timeK);
370 gtsam::Rot3 R_W_I_rollPitch;
371 static int alignImuCounter__ = -1;
373 double estimatedGravityMagnitude;
375 imuAttitudeRoll = R_W_I_rollPitch.roll();
376 imuAttitudePitch = R_W_I_rollPitch.pitch();
379 REGULAR_COUT <<
" Attitude of IMU is initialized. Determined Gravity Magnitude: " << estimatedGravityMagnitude << std::endl;
381 REGULAR_COUT <<
" Estimated gravity magnitude from IMU is: " << estimatedGravityMagnitude << std::endl;
382 REGULAR_COUT <<
" This gravity is not used, because estimateGravityFromImu is set to false. Gravity set to "
384 gtsam::Vector3 gravityVector = gtsam::Vector3(0, 0,
graphConfigPtr_->gravityMagnitude_);
385 gtsam::Vector3 estimatedGravityVector = gtsam::Vector3(0, 0, estimatedGravityMagnitude);
386 gtsam::Vector3 gravityVectorError = estimatedGravityVector - gravityVector;
387 gtsam::Vector3 gravityVectorErrorInImuFrame = R_W_I_rollPitch.inverse().rotate(gravityVectorError);
388 graphMgrPtr_->getInitAccBiasReference() = gravityVectorErrorInImuFrame;
389 std::cout <<
YELLOW_START <<
"GMsf" <<
COLOR_END <<
" Gravity error in IMU frame is: " << gravityVectorErrorInImuFrame.transpose()
404 REGULAR_COUT <<
GREEN_START <<
" Total initial IMU attitude is RPY (deg): " << T_W_I0.rotation().rpy().transpose() * (180.0 / M_PI)
411 graphMgrPtr_->initPoseVelocityBiasGraph(timeStamp_k, T_W_I0, T_O_I0, imuAngularVelocity);
414 gtsam::Pose3 T_W_I0_opt =
graphMgrPtr_->getOptimizedGraphState().navState().pose();
416 <<
" INITIAL POSE of IMU in world frame after first optimization, x,y,z (m): " << T_W_I0_opt.translation().transpose()
417 <<
", RPY (deg): " << T_W_I0_opt.rotation().rpy().transpose() * (180.0 / M_PI) <<
COLOR_END << std::endl;
428 REGULAR_COUT <<
" Thread for updating graph is ready." << std::endl;
429 double lastOptimizedTime = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
430 double currentTime = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
431 bool optimizedAtLeastOnce =
false;
433 bool optimizeGraphFlag =
false;
438 currentTime = std::chrono::duration<double>(std::chrono::system_clock::now().time_since_epoch()).count();
441 ((currentTime - lastOptimizedTime) > (1.0 /
graphConfigPtr_->minOptimizationFrequency_) && optimizedAtLeastOnce)) {
442 optimizeGraphFlag =
true;
443 lastOptimizedTime = currentTime;
444 optimizedAtLeastOnce =
true;
450 if (optimizeGraphFlag) {
454 std::this_thread::sleep_for(std::chrono::milliseconds(1));
461 const std::string& frame2,
const Eigen::Matrix3d& R_W_frame2) {
463 const Eigen::Isometry3d& T_frame2_frame1 =
staticTransformsPtr_->rv_T_frame1_frame2(frame2, frame1);
464 const Eigen::Vector3d& frame1_t_frame1_frame2 =
staticTransformsPtr_->rv_T_frame1_frame2(frame1, frame2).translation();
467 Eigen::Matrix3d R_W_frame1 = R_W_frame2 * T_frame2_frame1.rotation();
470 Eigen::Vector3d W_t_frame1_frame2 = R_W_frame1 * frame1_t_frame1_frame2;
473 return W_t_W_frame1 + W_t_frame1_frame2;
bool addCoreImuMeasurementAndGetState(const Eigen::Vector3d &linearAcc, const Eigen::Vector3d &angularVel, const double imuTimeK, std::shared_ptr< SafeIntegratedNavState > &returnPreIntegratedNavStatePtr, std::shared_ptr< SafeNavStateWithCovarianceAndBias > &returnOptimizedStateWithCovarianceAndBiasPtr, Eigen::Matrix< double, 6, 1 > &returnAddedImuMeasurements)
Main: IMU --------------------—.