Fix imu bias save bug

demos/solo_real_pov_estimation.cpp

@@ -494,11 +494,11 @@ int main (int argc, char **argv) {
         imu_bias = sen_imu->getIntrinsic()->getState();
         Vector7d extr_mocap_est; extr_mocap_est << sensor_pose->getP()->getState(), sensor_pose->getO()->getState();
 
-        mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(),   3*sizeof(double));
-        mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
-        mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(),   3*sizeof(double));
-        mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(),   6*sizeof(double));
-        mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(),   7*sizeof(double));
+        mempcpy(o_p_ob_fbk_carr+3*i, o_p_ob.data(),             3*sizeof(double));
+        mempcpy(o_q_b_fbk_carr +4*i, o_qvec_b.data(),           4*sizeof(double));
+        mempcpy(o_v_ob_fbk_carr+3*i, o_v_ob.data(),             3*sizeof(double));
+        mempcpy(imu_bias_fbk_carr+6*i, imu_bias.data(),         6*sizeof(double));
+        mempcpy(extr_mocap_fbk_carr+7*i, extr_mocap_est.data(), 7*sizeof(double));
 
         p_ob_fbk_v.push_back(o_p_ob);
         q_ob_fbk_v.push_back(o_qvec_b);
@@ -515,7 +515,7 @@ int main (int argc, char **argv) {
         problem->print(4,true,true,true);
         std::cout << report << std::endl;
     }
-
+    
     double o_p_ob_carr[3*N];
     double o_q_b_carr[4*N];
     double o_v_ob_carr[3*N];
@@ -527,8 +527,9 @@ int main (int argc, char **argv) {
         Vector4d o_qvec_i = state_est['O'];
         Vector3d o_v_oi = state_est['V'];
 
-        Vector6d imu_bias = sen_imu->getStateBlockDynamic('I', t_arr[i])->getState();
-        std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
+        auto sb = sen_imu->getStateBlockDynamic('I', t_arr[i]);
+        Vector6d imu_bias = sb->getState();
+        // std::cout << t_arr[i] << ", imu_bias: " << imu_bias.transpose() << std::endl;
         Vector3d i_omg_oi_unbiased = (i_omg_oi - imu_bias.tail<3>());
 
         Matrix3d o_R_i = Quaterniond(o_qvec_i).toRotationMatrix();
@@ -539,10 +540,10 @@ int main (int argc, char **argv) {
         // Vector3d o_p_ob = o_p_oi;
         // Vector3d o_v_ob = o_v_oi;
 
-        mempcpy(o_p_ob_carr+3*i, o_p_ob.data(),   3*sizeof(double));
-        mempcpy(o_q_b_carr +4*i, o_qvec_b.data(), 4*sizeof(double));
-        mempcpy(o_v_ob_carr+3*i, o_v_ob.data(),   3*sizeof(double));
-        mempcpy(imu_bias_carr+3*i, imu_bias.data(),   3*sizeof(double));
+        mempcpy(o_p_ob_carr+3*i, o_p_ob.data(),     3*sizeof(double));
+        mempcpy(o_q_b_carr +4*i, o_qvec_b.data(),   4*sizeof(double));
+        mempcpy(o_v_ob_carr+3*i, o_v_ob.data(),     3*sizeof(double));
+        mempcpy(imu_bias_carr+6*i, imu_bias.data(), 6*sizeof(double));
     }