/**
* get the measurement covariance at state
*/
void MapMeasurementFunction::getMeasurementCov(const RBIS & state, Eigen::Matrix3d & R_effective)
{
double xy[2] = { state.position()(0), state.position()(1) };
Eigen::Vector3d eulers = state.getEulerAngles();
double phipsi[2] = { eulers(0), eulers(2) };
R_effective = phi_psi_xy_cov_map->readValue(phipsi)->readValue(xy);
double theta_state = eulers(1);
double z_state = state.position()(2);
if (fabs(z_state - z_height) > 2.0) {
fprintf(stderr, "warning, querying measurement at different height than measurements were simulated, %f, %f\n",
z_state, z_height);
}
if (fabs(theta_state - theta) > bot_to_radians(20)) {
fprintf(stderr,
"warning, querying measurement at different theta (pitch) than measurements were simulated, %f, %f\n",
bot_to_degrees(theta_state), bot_to_degrees(theta));
}
}
void LCMFrontEnd::publishState(const RBIS & state, const RBIM & cov)
{
if (publish_pose) {
//publish the pose msg
rigid_body::pose_t pose_msg = state.getPose();
lcm_pub->publish(pose_channel, &pose_msg);
}
//publish filter state
if (publish_filter_state) {
mav::filter_state_t fs_msg = rbisCreateFilterStateMessageCPP(state, cov);
lcm_pub->publish(filter_state_channel, &fs_msg);
}
}
/**
* visualize will publish pose and laser messages along with GPF lcmgl visualization as measurments are computed, but it will be much slower.
*/
void MapMeasurementFunction::generateFromOctomap(const char * base_map_name, double mPP, double phi_max, double radPP,
lcm_t * lcm, BotParam * param, BotFrames * frames, double sigma_prior, double z_height, double vis_pause,
double max_laser_range)
{
bool visualize = vis_pause > 0;
double laser_sim_minNegLogLike;
// octomap::OcTree * sim_map = octomap::loadOctomap(base_map_name, &laser_sim_minNegLogLike);
octomap::OcTree * sim_map = new octomap::OcTree(base_map_name);
double minX, minY, minZ, maxX, maxY, maxZ;
sim_map->getMetricMin(minX, minY, minZ);
sim_map->getMetricMax(maxX, maxY, maxZ);
printf("\nmap bounds: [%.2f, %.2f, %.2f] - [%.2f, %.2f, %.2f] res: %f\n", minX, minY, minZ, maxX, maxY, maxZ,
sim_map->getResolution());
this->z_height = z_height;
LaserGPF laser_gpf = LaserGPF(lcm, param, frames);
// laser_gpf.motion_project = false;
laser_gpf.motion_mode = LaserGPF::motion_none;
//set the number of ranges for the simulator to the number after decimation then reset decimation
//FIXME also resest laser_gpf.spatial_decimation ?
// int nranges = NUM_HOKUYO_RANGES / ((double) laser_gpf.beam_skip); //put up with integer roundoff since it's big
// laser_gpf.beam_skip = 1;
//FIXME this right abe?
// int numHeightBeamsStart = laser_gpf.laser_projector->heightDownRegion[1];
// int numHeightBeamsEnd = laser_gpf.laser_projector->heightUpRegion[1] - laser_gpf.laser_projector->heightUpRegion[0];
int numHeightBeamsStart = 20;
int numHeightBeamsEnd = 0;
laser_util::LaserSim3D laser_sim = laser_util::LaserSim3D(sim_map, param, frames, "sim_laser");
double xy0[2] = { minX, minY };
double xy1[2] = { maxX, maxY };
double phi_psi_0[2];
double phi_psi_1[2];
phi_psi_0[0] = -phi_max;
phi_psi_1[0] = phi_max + radPP;
phi_psi_0[1] = -M_PI + radPP / 2.0;
phi_psi_1[1] = M_PI + radPP / 2.0;
xy_max_information = new occ_map::FloatPixelMap(xy0, xy1, mPP, 0);
xy_min_information = new occ_map::FloatPixelMap(xy0, xy1, mPP, INFINITY);
int x_dim = xy_max_information->dimensions[0];
int y_dim = xy_max_information->dimensions[1];
phi_psi_xy_cov_map = new CovPixelPixelMap(phi_psi_0, phi_psi_1, radPP, NULL);
phi_psi_xy_information_map = new FloatPixelPixelMap(phi_psi_0, phi_psi_1, radPP, 0);
int phi_dim = phi_psi_xy_cov_map->dimensions[0];
int psi_dim = phi_psi_xy_cov_map->dimensions[1];
int num_cells = x_dim * y_dim * phi_dim * psi_dim;
printf("Creating measurment map, x_dim = %d, y_dim = %d, phi_dim = %d, psi_dim = %d, %d total cells to evaluate\n",
x_dim, y_dim, phi_dim, psi_dim, num_cells);
printf("phi: min=%f, max=%f\npsi: min=%f, max=%f\n",
bot_to_degrees(phi_psi_xy_cov_map->xy0[0]), bot_to_degrees(phi_psi_xy_cov_map->xy1[0]),
bot_to_degrees(phi_psi_xy_cov_map->xy0[1]), bot_to_degrees(phi_psi_xy_cov_map->xy1[1]));
double x, y, phi, psi;
int phipsi_spatial_inds[2], xy_spatial_inds[2];
int phipsi_ind, xy_ind;
double phipsi_loc[2];
double xy_loc[2];
RBIS state = RBIS();
BotTrans bot_trans;
Eigen::VectorXd z_effective;
Eigen::MatrixXd R_effective;
RBIM prior_cov = RBIM::Zero();
prior_cov.block<3, 3>(RBIS::position_ind, RBIS::position_ind) = sigma_prior * Eigen::Matrix3d::Identity();
int num_completed = 0;
Eigen::Vector3d eulers;
for (int i = 0; i < phi_dim; i++) {
for (int j = 0; j < psi_dim; j++) {
phipsi_spatial_inds[0] = i;
phipsi_spatial_inds[1] = j;
phipsi_ind = phi_psi_xy_cov_map->getInd(phipsi_spatial_inds);
phi_psi_xy_cov_map->indToLoc(phipsi_ind, phipsi_loc);
CovPixelMap * xy_cov_map = new CovPixelMap(xy0, xy1, mPP);
occ_map::FloatPixelMap * xy_information_map = new occ_map::FloatPixelMap(xy0, xy1, mPP, 0);
phi_psi_xy_cov_map->writeValue(phipsi_spatial_inds, xy_cov_map);
phi_psi_xy_information_map->writeValue(phipsi_spatial_inds, xy_information_map);
for (int k = 0; k < x_dim; k++) {
for (int l = 0; l < y_dim; l++) {
xy_spatial_inds[0] = k;
xy_spatial_inds[1] = l;
xy_ind = xy_cov_map->getInd(xy_spatial_inds);
xy_cov_map->indToLoc(xy_ind, xy_loc);
//add .5 pixel res to all of them except roll
state.position()(0) = xy_loc[0];
state.position()(1) = xy_loc[1];
state.position()(2) = z_height;
eulers << phipsi_loc[0], 0, phipsi_loc[1];
state.setQuatEulerAngles(eulers);
state.getBotTrans(&bot_trans);
const bot_core_planar_lidar_t * laser_msg = laser_sim.simulate(&bot_trans);
bot_core::planar_lidar_t * laser_msg_cpp = new bot_core::planar_lidar_t;
laser_msg_cpp->intensities = std::vector<float>(laser_msg->nintensities);
laser_msg_cpp->nintensities = laser_msg->nintensities;
memcpy(&laser_msg_cpp->intensities[0], &laser_msg->intensities[0], laser_msg_cpp->nintensities * sizeof(float));
laser_msg_cpp->ranges = std::vector<float>(laser_msg->nranges);
laser_msg_cpp->nranges = laser_msg->nranges;
memcpy(&laser_msg_cpp->ranges[0], &laser_msg->ranges[0], laser_msg_cpp->nranges * sizeof(float));
laser_msg_cpp->rad0 = laser_msg->rad0;
laser_msg_cpp->radstep = laser_msg->radstep;
laser_msg_cpp->utime = laser_msg->utime;
laser_gpf.getMeasurement(state, prior_cov, laser_msg_cpp, z_effective, R_effective);
delete laser_msg_cpp;
double information_gain = R_effective.inverse().trace();
xy_cov_map->writeValue(xy_spatial_inds, R_effective);
xy_information_map->writeValue(xy_spatial_inds, information_gain);
if (information_gain > xy_max_information->readValue(xy_spatial_inds)) {
xy_max_information->writeValue(xy_spatial_inds, information_gain);
}
if (information_gain < xy_min_information->readValue(xy_spatial_inds)) {
xy_min_information->writeValue(xy_spatial_inds, information_gain);
}
if (visualize) {
rigid_body_pose_t pose;
state.getPose(&pose);
rigid_body_pose_t_publish(lcm, "STATE_ESTIMATOR_POSE", &pose);
bot_core_planar_lidar_t_publish(lcm, "LASER", laser_msg);
pronto_indexed_measurement_t * gpf_msg = gpfCreateLCMmsg(laser_gpf.laser_gpf_measurement_indices,
z_effective, R_effective);
gpf_msg->utime = 0;
gpf_msg->state_utime = 0;
pronto_indexed_measurement_t_publish(lcm, "GPF_MEASUREMENT", gpf_msg);
pronto_indexed_measurement_t_destroy(gpf_msg);
usleep(vis_pause * 1e6);
}
num_completed++;
fprintf(stdout, "\r evaluated %d/%d", num_completed, num_cells);
}
}
}
}
delete sim_map;
}
