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);
}
