void bot_trans_print_trans(const BotTrans * tran) { double rpy[3]; bot_quat_to_roll_pitch_yaw(tran->rot_quat, rpy); printf("t=(%f %f %f) rpy=(%f,%f,%f)", tran->trans_vec[0], tran->trans_vec[1], tran->trans_vec[2], bot_to_degrees(rpy[0]), bot_to_degrees(rpy[1]), bot_to_degrees(rpy[2])); }
/** * 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)); } }
static void frames_update_handler(BotFrames *bot_frames, const char *frame, const char * relative_to, int64_t utime, void *user) { RendererFrames *self = (RendererFrames *) user; BotTrans tran; int activeSensorNum = bot_gtk_param_widget_get_enum(self->pw, PARAM_FRAME_SELECT); if (activeSensorNum > 0) { const char * activeSensorName = self->frameNames[activeSensorNum]; if (strcmp(frame, activeSensorName) == 0) { const char * relative_to = bot_frames_get_relative_to(self->frames, activeSensorName); bot_frames_get_trans(self->frames, activeSensorName, relative_to, &tran); double rpy[3]; bot_quat_to_roll_pitch_yaw(tran.rot_quat, rpy); bot_gtk_param_widget_set_double(self->pw, PARAM_X, tran.trans_vec[0]); bot_gtk_param_widget_set_double(self->pw, PARAM_Y, tran.trans_vec[1]); bot_gtk_param_widget_set_double(self->pw, PARAM_Z, tran.trans_vec[2]); bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(rpy[0])); bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(rpy[1])); bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(rpy[2])); self->updating = 0; } } bot_viewer_request_redraw(self->viewer); }
/** * 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; }
static void on_param_widget_changed(BotGtkParamWidget *pw, const char *name, void *user) { RendererFrames *self = (RendererFrames*) user; if (self->updating) { return; } BotViewer *viewer = self->viewer; int activeSensorNum = bot_gtk_param_widget_get_enum(pw, PARAM_FRAME_SELECT); if (!strcmp(name, PARAM_FRAME_SELECT)) { if (activeSensorNum > 0) { self->updating = 1; bot_viewer_set_status_bar_message(self->viewer, "Modify Calibration relative to %s", bot_frames_get_relative_to( self->frames, self->frameNames[activeSensorNum])); bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 1); bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 1); bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 1); bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 1); bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 1); bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 1); bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 1); frames_update_handler(self->frames, self->frameNames[activeSensorNum], bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum]), bot_timestamp_now(), self); } else { bot_viewer_set_status_bar_message(self->viewer, ""); bot_gtk_param_widget_set_enabled(self->pw, PARAM_X, 0); bot_gtk_param_widget_set_enabled(self->pw, PARAM_Y, 0); bot_gtk_param_widget_set_enabled(self->pw, PARAM_Z, 0); bot_gtk_param_widget_set_enabled(self->pw, PARAM_ROLL, 0); bot_gtk_param_widget_set_enabled(self->pw, PARAM_PITCH, 0); bot_gtk_param_widget_set_enabled(self->pw, PARAM_YAW, 0); bot_gtk_param_widget_set_enabled(self->pw, PARAM_SAVE, 0); } } else if (!strcmp(name, PARAM_SAVE) && activeSensorNum > 0) { char save_fname[1024]; sprintf(save_fname, "manual_calib_%s.cfg", self->frameNames[activeSensorNum]); fprintf(stderr, "saving params to: %s\n", save_fname); FILE * f = fopen(save_fname, "w"); double pos[3]; pos[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X); pos[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y); pos[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z); double rpy[3]; rpy[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL); rpy[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH); rpy[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_YAW); double rpy_rad[3]; for (int i = 0; i < 3; i++) rpy_rad[i] = bot_to_radians(rpy[i]); double quat[4]; bot_roll_pitch_yaw_to_quat(rpy_rad, quat); double rod[3]; bot_quat_to_rodrigues(quat, rod); fprintf(f, "" "%s {\n" "position = [%f, %f, %f];\n" "rpy = [%f, %f, %f];\n" "relative_to = \"%s\";\n" "}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rpy[0], rpy[1], rpy[2], bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum])); fprintf(f, "" "\n" "%s {\n" "position = [%f, %f, %f];\n" "rodrigues = [%f, %f, %f];\n" "relative_to = \"%s\";\n" "}", self->frameNames[activeSensorNum], pos[0], pos[1], pos[2], rod[0], rod[1], rod[2], bot_frames_get_relative_to(self->frames, self->frameNames[activeSensorNum])); fclose(f); bot_viewer_set_status_bar_message(self->viewer, "Calibration saved to %s", save_fname); } else if (activeSensorNum > 0) { self->updating = 1; BotTrans curr; curr.trans_vec[0] = bot_gtk_param_widget_get_double(self->pw, PARAM_X); curr.trans_vec[1] = bot_gtk_param_widget_get_double(self->pw, PARAM_Y); curr.trans_vec[2] = bot_gtk_param_widget_get_double(self->pw, PARAM_Z); double rpy[3]; rpy[0] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_ROLL)); rpy[1] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_PITCH)); rpy[2] = bot_to_radians(bot_gtk_param_widget_get_double(self->pw, PARAM_YAW)); bot_roll_pitch_yaw_to_quat(rpy, curr.rot_quat); if (fabs(rpy[0]) > M_PI || fabs(rpy[1]) > M_PI || fabs(rpy[2]) > M_PI) { bot_gtk_param_widget_set_double(self->pw, PARAM_ROLL, bot_to_degrees(bot_mod2pi(rpy[0]))); bot_gtk_param_widget_set_double(self->pw, PARAM_PITCH, bot_to_degrees(bot_mod2pi(rpy[1]))); bot_gtk_param_widget_set_double(self->pw, PARAM_YAW, bot_to_degrees(bot_mod2pi(rpy[2]))); } //and update the link const char * frame_name = self->frameNames[activeSensorNum]; const char * relative_to = bot_frames_get_relative_to(self->frames, frame_name); bot_frames_update_frame(self->frames, frame_name, relative_to, &curr, bot_timestamp_now()); } bot_viewer_request_redraw(self->viewer); }