diff --git a/demos/solo_real_pov_estimation.cpp b/demos/solo_real_pov_estimation.cpp index 310f973c0346d4da0db46f17f120f376750d50c7..f52ae06bb3f9ca1379b6171f42485e81c53f6b69 100644 --- a/demos/solo_real_pov_estimation.cpp +++ b/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)); }