void compute_mean(SE3Type & mean_pose, Vector6 & mean_velocity) { mean_velocity.setZero(); for (int i = 0; i < 25; i++) { mean_velocity += sigma_velocity[i]; } mean_velocity /= 25.0f; mean_pose = sigma_pose[0]; Vector6 delta; const static int max_iterations = 1000; int iterations = 0; do { delta.setZero(); for (int i = 0; i < 25; i++) { delta += SE3Type::log(mean_pose.inverse() * sigma_pose[i]); } delta /= 25.0f; mean_pose *= SE3Type::exp(delta); iterations++; } while (delta.array().abs().maxCoeff() > Sophus::SophusConstants<_Scalar>::epsilon() && iterations < max_iterations); }
void compute_mean_and_covariance() { compute_mean(pose, velocity); covariance.setZero(); for (int i = 0; i < 25; i++) { Vector12 eps; eps.template head<6>() = SE3Type::log( pose.inverse() * sigma_pose[i]); eps.template tail<6>() = sigma_velocity[i] - velocity; covariance += eps * eps.transpose(); } covariance /= 2; }
void pose1Callback( const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) { Eigen::Quaterniond orientation; Eigen::Vector3d position; tf::pointMsgToEigen(msg->pose.pose.position, position); tf::quaternionMsgToEigen(msg->pose.pose.orientation, orientation); SE3Type pose(orientation, position); SE3Type transformed_pose = T_imu_cam.inverse() * pose; std::cout<<"Pose1 Position"<<"\n"<<transformed_pose.translation(); Matrix6 noise(&msg->pose.covariance[0]); std::cout<<"Noise "<<"\n"<<noise<<std::endl; ukf->measurePose( pose,noise); }