float get_initial_range_guess(float bearing, float heading, uint8_t power){
int8_t bestS = (6-((int8_t)ceilf((3.0*bearing)/M_PI)))%6;
float alpha = pretty_angle(bearing - basis_angle[bestS]); //alpha using infinite approximation
int8_t bestE = (6-((int8_t)ceilf((3.0*(bearing-heading-M_PI))/M_PI)))%6;
float beta = pretty_angle(bearing - heading - basis_angle[bestE] - M_PI); //beta using infinite approximation
//printf("(alpha: %f, sensor %u)\r\n", rad_to_deg(alpha), bestS);
if((alpha > M_PI_2) || (alpha < -M_PI_2)){
printf("ERROR: alpha out of range (alpha: %f, sensor %u)\r\n", rad_to_deg(alpha), bestS);
return 0;
}
if((beta > M_PI_2) || (beta < -M_PI_2)){
printf("ERROR: beta out of range (beta: %f, emitter %u)\r\n", beta, bestE);
return 0;
}
//printf("(beta: %f, emitter %u)\r\n", rad_to_deg(beta), bestE);
// expected contribution (using infinite distance approximation)
float amplitude;
float exp_con = sensor_model(alpha)*emitter_model(beta);
if(exp_con > 0) amplitude = brightMeas[bestE][bestS]/exp_con;
else{
printf("ERROR: exp_con (%f) is negative (or zero)!\r\n", exp_con);
return 0;
}
//printf("amp_for_inv: %f\t",amplitude);
float rMagEst = inverse_amplitude_model(amplitude, power);
float RX = rMagEst*cos(bearing)+DROPLET_RADIUS*(bearingBasis[bestS][0]-headingBasis[bestE][0]);
float RY = rMagEst*sin(bearing)+DROPLET_RADIUS*(bearingBasis[bestS][1]-headingBasis[bestE][1]);
float rangeEst = hypotf(RX,RY);
return rangeEst;
}
double getPositionFromWall(UNICYCLE_CONTROLLER *uc) {
sonar = ecrobot_get_sonar_sensor(NXT_PORT_S4);
if(sonar < 0)
uc->cPos = uc->pPos;
else
uc->cPos = 0.01*sonar;
return sensor_model(uc);
}
void estimation(int observationLeft, int observationRight) {
// we use the previous_localization to do the prediction
// and store the results in the current_localization
int current_state;
for(current_state = 0; current_state < nb_state; current_state++)
{
current_localization[current_state] = sensor_model(observationLeft, observationRight, current_state) * current_localization[current_state];
}
//estimation: confrontation between prediction and observation
}// estimation
void Simulation::move(VelocityControl *control) {
if (!m_motionModel || !m_sensorModel) {
return;
}
pfcpp::OdometryMotionModelSampler motion_model(m_motionModel->params());
pfcpp::BeamSensorModel sensor_model(
{m_sensorModel->maxRange(),
{m_sensorModel->a0(), m_sensorModel->a1(), m_sensorModel->a2(), m_sensorModel->a3()},
m_sensorModel->sigma(),
m_sensorModel->lambda()});
pfcpp::VelocityMotionModelSampler movement;
pimpl->robot_pos = movement(pimpl->robot_pos, *control);
std::vector<double> actual_measument;
std::vector<pfcpp::maps::ShapesMap::Position> sense_points;
std::tie(actual_measument, sense_points) = flatworld::measurement_with_coords(
pimpl->robot_pos, pimpl->map, m_mcl->numberOfBeams(), m_sensorModel->maxRange());
auto expected_measument = [&](auto p) {
return flatworld::measurement(p, pimpl->map, m_mcl->numberOfBeams(),
m_sensorModel->maxRange());
};
pimpl->pf([&](auto p) { return sensor_model(actual_measument, expected_measument(p)); },
[&](auto p) { return motion_model(p, movement(p, *control)); });
m_sensorBeams.clear();
for (auto p : sense_points) {
m_sensorBeams.push_back(std::make_shared<PointW>(QPointF{std::get<0>(p), std::get<1>(p)}));
}
m_currentPose->setPosition({pimpl->robot_pos.x, pimpl->robot_pos.y});
updateParticlesList();
emit updated();
}
// we do all the heavy lifting from here, sampleMotion just uses instance varialbes cov and mu
void MotionModelScanMatcher::setScans(const sensor_msgs::LaserScan reference_scan,
const sensor_msgs::LaserScan curr_scan) {
// ROS_INFO("[MotionModel::MotionModelScanMatcher]\tentering setScans: reference_scan.ranges.size()=%d",reference_scan.ranges.size());
// construct a sensor model from reference_scan
// first turn reference scan into occupancy grid for compatibility
nav_msgs::OccupancyGrid reference_scan_map;
ROS_INFO("[MotionModelScanMatcher::setScans]\tocc_xy_grid_dim=%d",occ_xy_grid_dim);
laserScanToOccupancyGrid(reference_scan, reference_scan_map);
ros::Time curr_time = ros::Time::now();
visualizeReferenceScanMap(reference_scan_map);
SensorModel sensor_model(reference_scan_map);
//sensor_model->visualizeLikelihoodMap(marker_pub,occ_xy_grid_dim, occ_xy_grid_dim,xy_grid_resolution);
sensor_model.updateScan(curr_scan);
// translation constants
// these describe the translation of the pose_grid ORIGIN wrt to the occupancy map ORIGIN
RobotPose pose_grid_center = odom_offset.asPose();
visualizeOdomOffset();
pose_grid_offset_x = (pose_grid_center.x / x_step) + (occ_x_dim - pose_x_dim) / 2;
pose_grid_offset_y = (pose_grid_center.y / y_step) + (occ_y_dim - pose_y_dim) / 2;
ROS_INFO("[MotionModelScanMatcher::setScans]\tpose_grid_offset_x=%f, pose_grid_offset_y=%f",pose_grid_offset_x, pose_grid_offset_y);
// construct grid datastructure, init cells to 1.0
PoseGrid sensor_model_grid(pose_theta_dim, pose_y_dim, pose_x_dim);
for(int i = 0; i < sensor_model_grid.num_cells; i++) sensor_model_grid.grid[i] = -DBL_MAX;
int grid_sample_rate = 4;
int scan_sample_rate = 1; // these are actually 1/rate, i.e number of scans/pixels to skip
sensor_model.calcGridProbs(&sensor_model_grid, pose_grid_offset_x, pose_grid_offset_y, grid_sample_rate, scan_sample_rate);
//computeSensorModelGrid(sensor_model_grid,sensor_model);
PoseGrid motion_model_grid(pose_theta_dim, pose_y_dim, pose_x_dim);
computeMotionModelGrid(motion_model_grid);
PoseGrid combined_grid(pose_theta_dim, pose_y_dim, pose_x_dim);
double max_val;
max_val = sensor_model_grid.grid[0] + motion_model_grid.grid[0];
bool use_motion_model = true;
int max_ind = -1;
double curr;
for (int i = 0; i < combined_grid.num_cells; i++) {
//ROS_INFO("[MotionModelScanMatcher::setScans]\tsensor_model:%f, motion_model:%f",sensor_model_grid.grid[i], motion_model_grid.grid[i]);
if(sensor_model_grid.grid[i] == -DBL_MAX){
combined_grid.grid[i] = -DBL_MAX;
continue;
}
if (use_motion_model) {
curr = sensor_model_grid.grid[i] + motion_model_grid.grid[i];
} else {
curr = sensor_model_grid.grid[i];
}
if (curr == 0) {
ROS_WARN("[MotionModelScanMatcher::setScans]\tsetting grid[%d] = 0.0",i);
}
combined_grid.grid[i] = curr;
if (curr > max_val) {
max_val = curr;
max_ind = i;
}
}
visualizePoseGrid(combined_grid, curr_time, grid_sample_rate);
max_pose = combined_grid.getIndexPose(max_ind);
max_pose.x = max_pose.x * x_step;
max_pose.y = max_pose.y * y_step;
max_pose.theta = max_pose.theta * theta_step;
//ROS_INFO("[MotionModelScanMatcher::setScans]\tmax_ind : %d, max_pose : (%f,%f,%f)",max_ind, max_pose.x,max_pose.y,max_pose.theta);
// extract mode (mu) and covariance matrix
estimateCovariance(max_pose,combined_grid);
}
float range_estimate(uint8_t brightness_matrix[6][6], float range_upper_limit, float bearing, float heading, uint8_t power)
{
float alpha, beta;
float SEcontribution;
float sensorRXx, sensorRXy, sensorTXx, sensorTXy;
float calcRIJmag, calcRmag;
float calcRx, calcRy;
float range_matrix[6][6];
uint8_t maxBright = 0;
uint8_t maxE;
uint8_t maxS;
for(uint8_t e = 0; e < 6; e++)
{
for(uint8_t s = 0; s < 6; s++)
{
if(brightness_matrix[e][s]>maxBright)
{
maxBright = brightness_matrix[e][s];
maxE = e;
maxS = s;
}
if(brightness_matrix[e][s] > BASELINE_NOISE_THRESHOLD)
{
sensorRXx = DROPLET_SENSOR_RADIUS*basis[s][0];
sensorRXy = DROPLET_SENSOR_RADIUS*basis[s][1];
sensorTXx = DROPLET_SENSOR_RADIUS*basis[e][0] + range_upper_limit*cosf(bearing);
sensorTXy = DROPLET_SENSOR_RADIUS*basis[e][1] + range_upper_limit*sinf(bearing);
alpha = atan2f(sensorTXy-sensorRXy,sensorTXx-sensorRXx) - basis_angle[s];
beta = atan2f(sensorRXy-sensorTXy,sensorRXx-sensorTXx) - basis_angle[e] - heading;
alpha = pretty_angle(alpha);
beta = pretty_angle(beta);
//if((alpha < M_PI/2)&&(alpha > -M_PI/2))
//{
//if((beta < M_PI/2)&&(beta > -M_PI/2))
//{
SEcontribution = sensor_model(alpha)*emitter_model(beta);
calcRIJmag = inverse_amplitude_model(brightness_matrix[e][s]/SEcontribution, power);
calcRx = calcRIJmag*cosf(alpha) + DROPLET_SENSOR_RADIUS*(basis[s][0] - basis[e][0]);
calcRy = calcRIJmag*sinf(alpha) + DROPLET_SENSOR_RADIUS*(basis[s][1] - basis[e][1]);
range_matrix[e][s] = sqrt(calcRx*calcRx + calcRy*calcRy);
continue;
//}
//}
}
range_matrix[e][s]=0;
}
}
//
//for(uint8_t e=0; e<6 ; e++){
//for(uint8_t s=0; s<6; s++){
//printf("%02.3f ",range_matrix[e][s]);
//}
//printf("\r\n");
//}
float range = range_matrix[maxE][maxS];
//printf("range from 0,0: %f\r\n",range_matrix[maxE][maxS]);
return range;
}
float get_initial_range_guess(float bearing, float heading, uint8_t sensor_total[6], uint8_t emitter_total[6], uint8_t brightness_matrix[6][6], uint8_t power)
{
uint8_t i, e, s, best_e, best_s;
uint16_t biggest_e_val = 0;
uint16_t biggest_s_val = 0;
float alpha, beta;
float EC, amplitude;
float inital_guess;
for(i = 0; i < 6; i++)
{
if(emitter_total[i] > biggest_e_val)
{
best_e = i;
biggest_e_val = emitter_total[best_e];
}
if(sensor_total[i] > biggest_s_val)
{
best_s = i;
biggest_s_val = sensor_total[best_s];
}
}
/*printf("BEST SENSOR: %u\r\n", best_s);
printf("BEST EMITTER: %u\r\n", best_e);*/
// find alpha using infinite approximation
alpha = bearing - atan2f(basis[best_s][1],basis[best_s][0]); // TODO: dont use atan2 for this simple task
alpha = pretty_angle(alpha);
// find beta using infinite approximation
beta = bearing - heading - atan2f(basis[best_e][1],basis[best_e][0]) - M_PI; // TODO: dont use atan2 for this simple task
beta = pretty_angle(beta);
// expected contribution (using infinite distance approximation)
if((alpha > M_PI/2)||(alpha < -M_PI/2)) // pi/2 = 1.5708
{
printf("ERROR: alpha out of range (alpha: %f, sensor %u)\r\n",alpha, best_s);
}
if((beta > M_PI/2)||(beta < -M_PI/2))
{
printf("ERROR: beta out of range (beta: %f, emitter %u)\r\n", beta, best_e);
}
EC = sensor_model(alpha)*emitter_model(beta);
if(EC > 0)
{
amplitude = brightness_matrix[best_e][best_s]/EC;
}
else
{
printf("ERROR: EC is negative (or zero)! oh, my (EC = %f)\r\n", EC);
}
return inverse_amplitude_model(amplitude, power) + 2*DROPLET_RADIUS;
}
float range_estimate(float init_range, float bearing, float heading, uint8_t power){
float range_matrix[6][6];
float sensorRXx, sensorRXy, sensorTXx, sensorTXy;
float alpha, beta, sense_emit_contr;
float calcRIJmag, calcRx, calcRy;
int16_t maxBright = -32768;
uint8_t maxE=0;
uint8_t maxS=0;
for(uint8_t e = 0; e < 6; e++){
for(uint8_t s = 0; s < 6; s++){
if(brightMeas[e][s]>maxBright){
maxBright = brightMeas[e][s];
maxE = e;
maxS = s;
}
if(brightMeas[e][s] > 0){
sensorRXx = DROPLET_RADIUS*getCosBearingBasis(0,s);
sensorRXy = DROPLET_RADIUS*getSinBearingBasis(0,s);
sensorTXx = DROPLET_RADIUS*cosf(basis_angle[e]+heading) + init_range*cosf(bearing);
sensorTXy = DROPLET_RADIUS*sinf(basis_angle[e]+heading) + init_range*sinf(bearing);
alpha = atan2f(sensorTXy-sensorRXy,sensorTXx-sensorRXx) - basis_angle[s];
beta = atan2f(sensorRXy-sensorTXy,sensorRXx-sensorTXx) - basis_angle[e] - heading;
alpha = pretty_angle(alpha);
beta = pretty_angle(beta);
sense_emit_contr = sensor_model(alpha)*emitter_model(beta);
//printf("sense_emit_contr: %f\r\n",sense_emit_contr);
if(sense_emit_contr>0){
calcRIJmag = inverse_amplitude_model(brightMeas[e][s]/sense_emit_contr, power);
}else{
calcRIJmag = 0;
}
calcRx = calcRIJmag*cosf(alpha) + sensorRXx - DROPLET_RADIUS*cosf(basis_angle[e]+heading);
calcRy = calcRIJmag*sinf(alpha) + sensorRXy - DROPLET_RADIUS*sinf(basis_angle[e]+heading);
range_matrix[e][s] = hypotf(calcRx, calcRy);
continue;
}
range_matrix[e][s]=0;
}
}
float rangeMatSubset[3][3];
float brightMatSubset[3][3];
float froebNormSquared=0;
for(uint8_t e = 0; e < 3; e++){
for(uint8_t s = 0; s < 3; s++){
uint8_t otherE = ((maxE+(e+5))%6);
uint8_t otherS = ((maxS+(s+5))%6);
rangeMatSubset[e][s] = range_matrix[otherE][otherS];
brightMatSubset[e][s] = (float)brightMeas[otherE][otherS];
froebNormSquared+=powf(brightMatSubset[e][s],2);
}
}
float froebNorm = sqrtf(froebNormSquared);
float range = 0;
for(uint8_t e = 0; e < 3; e++){
for(uint8_t s = 0; s < 3; s++){
range+= rangeMatSubset[e][s]*powf(brightMatSubset[e][s]/froebNorm,2);
}
}
//printf("R: %f\r\n", range);
//print_range_matrix(range_matrix);
//printf("\n");
return range;
}
int main(int argc,char** argv)
{
double value;
bool odom_laser_flag=false;
string meas,meas_id;
ifstream log_file; // input file stream for log file
istringstream log_stream; // stream to parse log values
mat laser_meas = zeros<mat>(n_laser_meas,1); // vector for laser measure
mat odom_meas = zeros<mat>(n_odom_meas,1); // vector for odom measure
mat map = parse_map(map_file_name); // map in matrix form
mat particles = initialize_particles(map); // particles as (x,y,p)
mat previous_pose = zeros<mat>(3,1);
log_file.open(log_file_name.c_str());
if(!log_file.is_open()){
cout<<"log file not open\n";
return 1;
}
// cout << particles << endl;
for(int j=0;j<=log_length ;j++)
{
getline(log_file,meas);
log_stream.str(meas + " ");
log_stream >> meas_id;
if(odom_laser_flag == false){
if(meas_id == "O"){
odom_laser_flag = true;
for(int i=0;i<n_odom_meas;i++){
log_stream >> value;
odom_meas(i,0) = value; }
if(j==1){
previous_pose(0,0) = odom_meas(0,0);
previous_pose(1,0) = odom_meas(1,0);
previous_pose(2,0) = odom_meas(2,0);
}
particles=motion_model(particles,odom_meas,previous_pose);
previous_pose(0,0) = odom_meas(0,0);
previous_pose(1,0) = odom_meas(1,0);
previous_pose(2,0) = odom_meas(2,0);
}
}
if(odom_laser_flag == true)
{
if(meas_id == "L"){
odom_laser_flag = false;
for(int i=0;i<n_laser_meas;i++){
log_stream >> value;
laser_meas(i,0) = value; }
odom_meas(0,0) = laser_meas(0,0);
odom_meas(1,0) = laser_meas(1,0);
odom_meas(2,0) = laser_meas(2,0);
particles=motion_model(particles,odom_meas,previous_pose);
particles=sensor_model(map,particles,laser_meas,j);
previous_pose(0,0) = laser_meas(0,0);
previous_pose(1,0) = laser_meas(1,0);
previous_pose(2,0) = laser_meas(2,0);
particles = resample(particles,j);
}
to_cvmat(map,particles,j);
}
