int
PotentialManager::populatePotential(Potential* aPot, bool random)
{
VSET& potVars=aPot->getAssocVariables();
for(VSET_ITER vIter=potVars.begin();vIter!=potVars.end(); vIter++)
{
double mean=0;
double cov=0;
INTDBLMAP* covar=NULL;
if(random)
{
if(globalMean_Rand.find(vIter->first)==globalMean_Rand.end())
{
cout <<"No var with id " << vIter->first << endl;
exit(0);
}
mean=globalMean_Rand[vIter->first];
covar=globalCovar_Rand[vIter->first];
}
else
{
if(globalMean.find(vIter->first)==globalMean.end())
{
cout <<"No var with id " << vIter->first << endl;
exit(0);
}
mean=globalMean[vIter->first];
covar=globalCovar[vIter->first];
}
aPot->updateMean(vIter->first,mean);
for(VSET_ITER uIter=vIter;uIter!=potVars.end();uIter++)
{
if(covar->find(uIter->first)==covar->end())
{
estimateCovariance(random,covar,vIter->first,uIter->first);
//cout <<"No var " << uIter->first << " in covariance of " << vIter->first << endl;
//exit(0);
}
double cval=(*covar)[uIter->first];
aPot->updateCovariance(vIter->first,uIter->first,cval);
aPot->updateCovariance(uIter->first,vIter->first,cval);
}
}
//aPot->makeValidJPD();
aPot->makeValidJPD(ludecomp,perm);
return 0;
}
// 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);
}
