aau-cns/mars_lib

MaRS: A Modular and Robust Sensor-Fusion Framework

mars_lib's Issues

bug: residual not stored in member variable of (pose) sensor class

@antithing recently pointed out that within the pose sensor class, the calculated residual is not stored within the member variable of the inherited UpdateSensorAbsClass:

mars_lib/source/mars/include/mars/sensors/pose/pose_sensor_class.h

Lines 202 to 204 in 18e9a92

	Eigen::MatrixXd res(res_p.rows() + res_r.rows(), 1);
	res << res_p, res_r;

Thus an access from outside the update function to the last residual is not possible.

This issue might also effect other sensor classes.

The issue was originally posted by @mascheiber in #9 (comment)

Concerning the residual printing, I just saw that this is a bug within the pose sensor class, where the value is only stored locally but not written to the residual_ member variable.

Thus thank you for pointing this out, will be providing a fix soon.

For now you could add the printing the in the pose_sensor_class.h, line 204:

mars_lib/source/mars/include/mars/sensors/pose/pose_sensor_class.h

Lines 202 to 204 in 18e9a92

	Eigen::MatrixXd res(res_p.rows() + res_r.rows(), 1);
	res << res_p, res_r;

Problems about world frame

As used in the project and specified in docs, the mars_lib uses ENU as world frame of core states.

Then, if we want to fuse imu with 6DOF poses which use NED as world frame, shouldn't we do some transformations to make them use consistent world frames?

I didn't see any transformation that fixes this kind of problems, like code in https://github.com/aau-cns/mars_lib/blob/18e9a924733efbb97de76328e1713b13153e9634/source/mars/include/mars/sensors/pose/pose_sensor_class.h#L195C1-L196C1

So, How to fuse different sensors which use different world frames?

Problems about Chi2 rejection

Thanks for your work.

About the example of mars_thl

Hi authors, thanks for your work!
I tried running the example provided in your code, but it prompted that the traj.csv and pose_sensor_1.csv file are missing the headers. So I added headers in these files in the order of variables written in the code. And I got the result below:
States Initialized:
p_wi: [ 0.0000000000 0.0000000000 5.0000000000 ]
v_wi: [ 0.0000000000 0.0000000000 0.0000000000 ]
q_wi: [ 1.0000000000 0.0000000000 0.0000000000 0.0000000000 ]
b_w: [ 0.0000000000 0.0000000000 0.0000000000 ]
b_a: [ 0.0000000000 0.0000000000 0.0000000000 ]
w_m: [ 0.0446263726 0.1522124562 9.6225157105 ]
a_m: [ 0.0112082519 0.0041435909 -0.0170691543 ]

Info: Filter was initialized
Info: Initialized [Pose] with [Given] Calibration at t=0.0200000000
Last State:
p_wi: [ 0.4011546192 0.0754448151 -0.0838039919 ]
v_wi: [ 0.5411666457 -1.3466671535 -1.6892457075 ]
q_wi: [ -0.2994304785 -0.5357607869 -0.1786263497 0.7690217129 ]
b_w: [ 1.5799786506 -0.0864311147 8.9297938184 ]
b_a: [ -1.3796940856 0.3320506584 -4.6791725388 ]
w_m: [ 4.2713690019 2.0292181981 8.5375555121 ]
a_m: [ -1.2525263620 -0.2494051525 -1.5072852095 ]

p_wi error [m]: [20947.2185273579 3518.1154389416 -8631.2358500692 ]
v_wi error [m/s]: [-15.3835529174 19.1372170633 -13.1444001735 ]
q_wi error [w,x,y,z]: [-0.4168798284 0.4702076899 0.2173425926 -0.7469124008 ]

This result is obviously unreasonable, and I found that the final ground truth values provided in the code also seems incorrect.
// Define final ground truth values
Eigen::Vector3d true_p_wi(-20946.817372738657, -3518.039994126535, 8631.1520460773336);
Eigen::Vector3d true_v_wi(15.924719563070044, -20.483884216740151, 11.455154466026718);
Eigen::Quaterniond true_q_wi(0.98996033625708202, 0.048830414166879263, -0.02917972697860232, -0.12939345742158029);

Initialization problem

Thanks for sharing.
I use this work fusing imu and GPS data, then aligning lidar points, when I test it using Kitti datasets, the first 50s result is wrong.
Does this work need static initialization or is something wrong?

Pose and IMU data fusion, adapting e2e example.

Hi, I am setting up a system to use pose data (from aruco markers) and fuse it with IMU data.

I have everything compiling and running, but when I print the filter result, it is very different from the aruco pose 'ground truth'.

Using this code to print teh data:

mars::BufferEntryType latest_result;
core_logic.buffer_.get_latest_sensor_handle_state(pose_sensor_sptr, &latest_result);
mars::PoseSensorStateType last_state = pose_sensor_sptr->get_state(latest_result.data_.sensor_);
std::cout << last_state.to_csv_string(latest_result.timestamp_.get_seconds()) << std::endl;

The results are:

13.0012650425 POSE (position x of aruco pose)

1688634078.3723161, 1.1928349011293802, 1.4330804194659952, 1.2492615835714926, 0.74193921720270184, -0.29832209671360393, 0.13098116616783731, -0.5859812784538041

13.0012650425 POSE (position x of aruco pose)

1688634078.417676, 1.1935848113331251, 1.4321541090035363, 1.2492461113616447, 0.74188893059537642, -0.29846200083580293, 0.13186565356675767, -0.58577529661684269

The relevant code is:

//mars setup:

std::vector<double> imu_n_w{ 0.013, 0.013, 0.013 };
std::vector<double> imu_n_bw{ 0.0013, 0.0013, 0.0013 };
std::vector<double> imu_n_a{ 0.083, 0.083, 0.083 };
std::vector<double> imu_n_ba{ 0.0083, 0.0083, 0.0083 };

// setup propagation sensor
std::shared_ptr<mars::ImuSensorClass> imu_sensor_sptr = std::make_shared<mars::ImuSensorClass>("IMU");

// setup the core definition
std::shared_ptr<mars::CoreState> core_states_sptr = std::make_shared<mars::CoreState>();
core_states_sptr.get()->set_propagation_sensor(imu_sensor_sptr);
core_states_sptr.get()->set_noise_std(Eigen::Vector3d(imu_n_w.data()), Eigen::Vector3d(imu_n_bw.data()),
Eigen::Vector3d(imu_n_a.data()), Eigen::Vector3d(imu_n_ba.data()));

// setup additional sensors
// Pose sensor
std::shared_ptr<mars::PoseSensorClass> pose_sensor_sptr = std::make_shared<mars::PoseSensorClass>("Pose", core_states_sptr);
pose_sensor_sptr->const_ref_to_nav_ = true;  // TODO is set here for now but will be managed by core logic in later versions

// Define measurement noise
Eigen::Matrix<double, 6, 1> pose_meas_std;
pose_meas_std << 0.02, 0.02, 0.02, 2 * (M_PI / 180), 2 * (M_PI / 180), 2 * (M_PI / 180);
pose_sensor_sptr->R_ = pose_meas_std.cwiseProduct(pose_meas_std);

// Define initial calibration and covariance
mars::PoseSensorData pose_init_cal;
pose_init_cal.state_.p_ip_ = Eigen::Vector3d::Zero();
pose_init_cal.state_.q_ip_ = Eigen::Quaterniond::Identity();

// The covariance should enclose the initialization with a 3 Sigma bound
Eigen::Matrix<double, 6, 1> std;
std << 0.1, 0.1, 0.1, (10 * M_PI / 180), (10 * M_PI / 180), (10 * M_PI / 180);
pose_init_cal.sensor_cov_ = std.cwiseProduct(std).asDiagonal();

pose_sensor_sptr->set_initial_calib(std::make_shared<mars::PoseSensorData>(pose_init_cal));



// create the CoreLogic and link the core states
mars::CoreLogic core_logic(core_states_sptr);

Then I am running a live camera and grabbing the camera pose from an aruco marker pnp solve.
II also grab the imu data, and run both through the filter.

sl::SensorsData sensors_data;
SensorsData::IMUData imu_data;


mars::BufferDataType dataPose;
bool frame = false;
bool dataGot = false;
double previousImuTimestamp = -1;
// double previousVideoTimestamp = -1;
double previousPoseTimestamp = -1;

std::thread sensThread([&]() {
    while (!exiting) {
        // Using sl::TIME_REFERENCE::CURRENT decouples this from Camera.grab() so we get full 400Hz sampling rate
        if (zed.getSensorsData(sensors_data, sl::TIME_REFERENCE::CURRENT) == sl::ERROR_CODE::SUCCESS) {
            constexpr double D2R = M_PI / 180.0; // Zed 2 uses degrees, convert to radians
            double timestamp = (double)sensors_data.imu.timestamp.getNanoseconds() / 1e9;
            if (timestamp != previousImuTimestamp) {
                auto angular_velocity = sensors_data.imu.angular_velocity;
                auto linear_acceleration = sensors_data.imu.linear_acceleration;
                gyro = Eigen::Vector3d(angular_velocity.x * D2R, angular_velocity.y * D2R, angular_velocity.z * D2R);
                accel = Eigen::Vector3d(linear_acceleration.x, linear_acceleration.y, linear_acceleration.z);

                imu_data = sensors_data.imu;
                sl::Transform imuPose = imu_data.pose;

                //grab and store
                GyroscopeDatum g;
                g.t = timestamp;
                g.v = gyro;
                AccelerometerDatum a;
                a.t = timestamp;
                a.v = accel;

                Eigen::Vector3d _acceleration(accel.x(), accel.y(), accel.z());
                Eigen::Vector3d _velocity(gyro.x(), gyro.y(), gyro.z());

                mars::BufferDataType data;
                data.set_sensor_data(std::make_shared<mars::IMUMeasurementType>(_acceleration, _velocity));

                core_logic.ProcessMeasurement(imu_sensor_sptr, timestamp, data);

                       
                if (frame == true)
                {
                    mars::BufferDataType dataP;

                    ReadLock w_lock(grabberLock);
                    dataP = dataPose;
                    w_lock.unlock();

                    frame = false;

                    core_logic.ProcessMeasurement(pose_sensor_sptr, timestamp, dataP);

                    dataGot = true;

       
                }

                if (!core_logic.core_is_initialized_)
                {
                    // Initialize the first time at which the propagation sensor occures
                    if (imu_sensor_sptr == core_logic.core_states_->propagation_sensor_)
                    {
                        Eigen::Vector3d p_wi_init(0, 0, 5);
                        Eigen::Quaterniond q_wi_init = Eigen::Quaterniond::Identity();
                        core_logic.Initialize(p_wi_init, q_wi_init);
                    }
                    else
                    {
                        continue;
                    }
                }

               
                       
                if (dataGot)
                {
                    // Repropagation after an out of order update can cause the latest state to be different from the current update
                    // sensor. Using get_latest_sensor_handle_state is the safest option.
                    mars::BufferEntryType latest_result;
                            core_logic.buffer_.get_latest_sensor_handle_state(pose_sensor_sptr, &latest_result);
                            mars::PoseSensorStateType last_state = pose_sensor_sptr->get_state(latest_result.data_.sensor_);
                            std::cout << last_state.to_csv_string(latest_result.timestamp_.get_seconds()) << std::endl;
                            
                        }

                    previousImuTimestamp = timestamp;
                }

            }
        }
        });


    double lastFrameRatePublish = -1;
    constexpr double PUBLISH_INTERVAL = 1.0;
    constexpr size_t WINDOW_FRAMES = 100;



    std::thread videoThread([&]() {
        while (!exiting) {
            cv::Mat draw;
            if (zed.grab() == sl::ERROR_CODE::SUCCESS) {
                double timestamp = (double)zed.getTimestamp(sl::TIME_REFERENCE::IMAGE).getNanoseconds() / 1e9;


                zed.retrieveImage(image_zed, sl::VIEW::LEFT_GRAY, sl::MEM::CPU);
                zed.retrieveImage(image_zedR, sl::VIEW::RIGHT_GRAY, sl::MEM::CPU);
                zed.retrieveImage(image_zedD, sl::VIEW::DEPTH, sl::MEM::CPU);
              
               
                CameraDatum c;
                c.t = timestamp;
                c.images.push_back(image_ocv.clone());
                c.images.push_back(image_ocvR.clone());
                c.depth = image_ocvD.clone();

                std::chrono::milliseconds ms = std::chrono::duration_cast<std::chrono::milliseconds>(
                    std::chrono::system_clock::now().time_since_epoch());
                c.ms = ms;
                         
                cv::cvtColor(image_ocv, draw, cv::COLOR_GRAY2RGB);

                cv::aruco::detectMarkers(draw, dictionary, markerCorners, markerIds, parameters, rejectedCandidates);             



                std::vector<cv::Vec3d> rvecs, tvecs;
                cv::aruco::estimatePoseSingleMarkers(markerCorners, 0.05, K, D, rvecs, tvecs);

                if (rvecs.size() > 0)
                {
                    cv::Mat R;
                    cv::Rodrigues(rvecs[0], R); 

                    R = R.t();  // calculate camera R matrix

                  //  cv::Mat camRvec;
                 //   Rodrigues(R, camRvec); // calculate camera rvec

                    cv::Mat camTvec = -R * tvecs[0]; // calculate camera translation vector
                    cv::Vec3d tvec(camTvec);
                    Eigen::Matrix3d mat;
                    cv2eigen(R, mat);
                    Eigen::Quaterniond EigenQuat(mat);


                    Eigen::Vector3d position(tvec(0), tvec(1), tvec(2));
                    Eigen::Quaterniond orientation(EigenQuat.w(), EigenQuat.x(), EigenQuat.y(), EigenQuat.z());
                    std::cout << position.x() << " POSE " << std::endl;
                    mars::BufferDataType data;
                    data.set_sensor_data(std::make_shared<mars::PoseMeasurementType>(position, orientation));
                 
                    WriteLock w_lock(grabberLock);
                    dataPose = data;
                    w_lock.unlock();

                    frame = true;                 
                }

                for (int i = 0; i < markerIds.size(); i++)
                {
                    cv::aruco::drawDetectedMarkers(draw, markerCorners, markerIds);
                    cv::drawFrameAxes(draw, K, D, rvecs[i], tvecs[i], 0.1);
                }

                cv::imshow("marker", draw);
                cv::waitKey(1);

                frameNumber++;               

                bNewFrame = true;


            }
         
        }

        });

I would expect the output from the filter and the pose to be similar, what am I doing wrong?

Thank you

Ubuntu 20.04 GCC9 googletest - Deprecated implicit copy

The new "-Wdeprecated-copy-dtor" flag, added within "-wextras" causes the googletest build to fail. This issue has been reported here and here.

Pose and IMU fusion

Hi, and thank you for making this code available.

I need to add IMU data into existing pose data (xyz / xyzw) from a SLAM system.

The goal is to smooth and predict data when tracking is lost.

Is this code suitable for that purpose? Is there an example that might help me get started?

Thanks!

Googletest, warning treated as error in debug build with GCC11

The previous Googletest version produces errors during the debug build with a newer GCC version (GCC 11) used by Ubuntu 22.04 currently used as runs-on: ubuntu-latest for Github workflows.

In file included from /home/runner/work/mars_lib/mars_lib/source/tests/googletest/googletest/src/gtest-all.cc:43:
/home/runner/work/mars_lib/mars_lib/source/tests/googletest/googletest/src/gtest-death-test.cc: In function ‘bool testing::internal::StackGrowsDown()’:
/home/runner/work/mars_lib/mars_lib/source/tests/googletest/googletest/src/gtest-death-test.cc:1008:24: error: ‘dummy’ may be used uninitialized [-Werror=maybe-uninitialized]
 1008 |   StackLowerThanAddress(&dummy, &result);
      |   ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~
/home/runner/work/mars_lib/mars_lib/source/tests/googletest/googletest/src/gtest-death-test.cc:998:6: note: by argument 1 of type ‘const void*’ to ‘void testing::internal::StackLowerThanAddress(const void*, bool*)’ declared here
  998 | void StackLowerThanAddress(const void* ptr, bool* result) {
      |      ^~~~~~~~~~~~~~~~~~~~~
/home/runner/work/mars_lib/mars_lib/source/tests/googletest/googletest/src/gtest-death-test.cc:1006:7: note: ‘dummy’ declared here
 1006 |   int dummy;
      |       ^~~~~
cc1plus: all warnings being treated as errors
gmake[2]: *** [source/tests/googletest/googlemock/CMakeFiles/gmock.dir/build.make:76: source/tests/googletest/googlemock/CMakeFiles/gmock.dir/__/googletest/src/gtest-all.cc.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:551: source/tests/googletest/googlemock/CMakeFiles/gmock.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2

Explain of `CoreState::CalcQSmallAngleApprox`

Hi authors, thanks for your work!
I read the code and it is clear and well documented.
There is a function CoreState::CalcQSmallAngleApprox in core_state_calc_q.cpp, could you explain how to generate it?

Asynchronous fusion of multi-sensor data

Hi and thanks for your work!
I want to study an asynchronous multi-camera system, is this code suitable for the purpose, is there any relevant case?

Weird result in `mars_thl_example.cpp`

Running the mars_thl_example (version: 0.1.0-1c08c9024dc3) I get the following output:

Trajectory File: traj.csv
Pose File: pose_sensor_1.csv
IMU Noise Parameter: 
imu_n_w: 	 [0.013 0.013 0.013  ]
imu_n_bw: 	 [0.0013 0.0013 0.0013  ]
imu_n_a: 	 [0.083 0.083 0.083  ]
imu_n_ba: 	 [0.0083 0.0083 0.0083  ]
Created: [IMU] Sensor
Created: [Pose] Sensor
Created: Buffer (Size=300)
Created: Buffer (Size=100)
Created: CoreLogic - Using MaRS Version: 0.1.0-1c08c9024dc3
Warning: Core is not initialized yet. Measurement is stored but not processed
States Initialized:
p_wi:	[ 0.0000000000 0.0000000000 5.0000000000 ]
v_wi:	[ 0.0000000000 0.0000000000 0.0000000000 ]
q_wi:	[ 1.0000000000 0.0000000000 0.0000000000 0.0000000000 ]
b_w:	[ 0.0000000000 0.0000000000 0.0000000000 ]
b_a:	[ 0.0000000000 0.0000000000 0.0000000000 ]
w_m:	[  0.0112082519  0.0041435909 -0.0170691543 ]
a_m:	[ 0.0446263726 0.1522124562 9.6225157105 ]

Info: Filter was initialized
Info: Initialized [Pose] with [Given] Calibration at t=0.0200000000	
Last State:
p_wi:	[ -0.0046029310  0.0059225648  0.2394782674 ]
v_wi:	[ -0.0132090009  0.0135288417 -0.0003068795 ]
q_wi:	[ 0.9662831594  0.1198218208 -0.2223775546  0.0498779543 ]
b_w:	[ -0.0020980574  0.0021448086  0.0019942386 ]
b_a:	[ -0.0076526597 -0.0083927667  0.0015927020 ]
w_m:	[ -1.2525263620 -0.2494051525 -1.5072852095 ]
a_m:	[ 4.2713690019 2.0292181981 8.5375555121 ]

p_wi error [m]: [20946.8127698077  3518.0459166914 -8630.9125678099 ]
v_wi error [m/s]: [-15.9379285640  20.4974130584 -11.4554613456 ]
q_wi error [w,x,y,z]: [0.9624679859 -0.1016644688  0.1740093591 -0.1817703354 ]

The error values are abnormally large as if the filtering wasn't set up/working properly, or am I doing something wrong?

I've noticed this is already referenced in the #6, where the out-of-the-box running was fixed. But wouldn't it also make sense that the default provided GT values in the example are the correct ones for the bundled dataset?