//send back the tangent
const Matrix& CycLiqCP3D :: getTangent( )
{
// matrix to tensor mapping
// Matrix Tensor
// ------- -------
// 0 0 0
// 1 1 1
// 2 2 2
// 3 0 1 ( or 1 0 )
// 4 1 2 ( or 2 1 )
// 5 2 0 ( or 0 2 )
int ii, jj ;
int i, j, k, l ;
for ( ii = 0; ii < 6; ii++ ) {
for ( jj = 0; jj < 6; jj++ ) {
index_map( ii, i, j ) ;
index_map( jj, k, l ) ;
tangent_matrix(ii,jj) = tangent[i][j][k][l] ;
} //end for j
} //end for i
return tangent_matrix ;
}
//send back the tangent
const Matrix& J2AxiSymm :: getInitialTangent( )
{
// matrix to tensor mapping
// Matrix Tensor
// ------- -------
// 0 0 0
// 1 1 1
// 2 2 2
// 3 0 1 ( or 1 0 )
this->doInitialTangent();
int ii, jj ;
int i, j, k, l ;
for ( ii = 0; ii < 4; ii++ ) {
for ( jj = 0; jj < 4; jj++ ) {
index_map( ii, i, j ) ;
index_map( jj, k, l ) ;
tangent_matrix(ii,jj) = initialTangent[i][j][k][l] ;
} //end for j
} //end for i
return tangent_matrix ;
}
//send back the tangent
const Matrix& J2ThreeDimensional :: getInitialTangent( )
{
// matrix to tensor mapping
// Matrix Tensor
// ------- -------
// 0 0 0
// 1 1 1
// 2 2 2
// 3 0 1 ( or 1 0 )
// 4 1 2 ( or 2 1 )
// 5 2 0 ( or 0 2 )
int ii, jj ;
int i, j, k, l ;
this->doInitialTangent();
for ( ii = 0; ii < 6; ii++ ) {
for ( jj = 0; jj < 6; jj++ ) {
index_map( ii, i, j ) ;
index_map( jj, k, l ) ;
tangent_matrix(ii,jj) = initialTangent[i][j][k][l] ;
} //end for j
} //end for i
return tangent_matrix ;
}
// set up for initial elastic
void J2Plasticity :: doInitialTangent( )
{
int ii,jj,i,j,k,l;
//compute the deviatoric strains
for ( ii = 0; ii < 6; ii++ ) {
for ( jj = 0; jj < 6; jj++ ) {
index_map( ii, i, j ) ;
index_map( jj, k, l ) ;
//elastic terms
initialTangent[i][j][k][l] = bulk * IbunI[i][j][k][l] ;
initialTangent[i][j][k][l] += (2.0*shear) * IIdev[i][j][k][l] ;
//minor symmetries
//minor symmetries
initialTangent [j][i][k][l] = initialTangent[i][j][k][l] ;
initialTangent [i][j][l][k] = initialTangent[i][j][k][l] ;
initialTangent [j][i][l][k] = initialTangent[i][j][k][l] ;
} // end for jj
} // end for ii
return ;
}
int main()
{
// Construct an arrangement of seven intersecting line segments.
Point_2 p1(1, 1), p2(1, 4), p3(2, 2), p4(3, 7), p5(4, 4), p6(7, 1), p7(9, 3);
Ex_arrangement arr;
insert(arr, Segment_2(p1, p6));
insert(arr, Segment_2(p1, p4)); insert(arr, Segment_2(p2, p6));
insert(arr, Segment_2(p3, p7)); insert(arr, Segment_2(p3, p5));
insert(arr, Segment_2(p6, p7)); insert(arr, Segment_2(p4, p7));
// Create a mapping of the arrangement faces to indices.
Face_index_map index_map(arr);
// Perform breadth-first search from the unbounded face, using the event
// visitor to associate each arrangement face with its discover time.
unsigned int time = 0;
boost::breadth_first_search(Dual_arrangement(arr), arr.unbounded_face(),
boost::vertex_index_map(index_map).visitor
(boost::make_bfs_visitor
(stamp_times(Face_property_map(), time,
boost::on_discover_vertex()))));
// Print the discover time of each arrangement face.
Ex_arrangement::Face_iterator fit;
for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit) {
std::cout << "Discover time " << fit->data() << " for ";
if (fit != arr.unbounded_face()) {
std::cout << "face ";
print_ccb<Ex_arrangement>(fit->outer_ccb());
}
else std::cout << "the unbounded face." << std::endl;
}
return 0;
}
void AbstractParameterisedSystem<VECTOR>::CheckParametersOnLoad(const std::vector<double>& rParameters, const std::vector<std::string>& rParameterNames)
{
if (GetVectorSize(mParameters) != rGetParameterNames().size())
{
// Subclass constructor didn't give default values, so we need the archive to provide them all
if (rParameterNames.size() != rGetParameterNames().size())
{
EXCEPTION("Number of ODE parameters in archive does not match number in class.");
}
CreateVectorIfEmpty(mParameters,rGetParameterNames().size());
}
// Check whether the archive specifies parameters that don't appear in this class,
// and create a map from archive index to local index
std::vector<unsigned> index_map(rParameterNames.size());
for (unsigned i=0; i<rParameterNames.size(); ++i)
{
index_map[i] = find(rGetParameterNames().begin(), rGetParameterNames().end(), rParameterNames[i])
- rGetParameterNames().begin();
if (index_map[i] == rGetParameterNames().size())
{
EXCEPTION("Archive specifies a parameter '" + rParameterNames[i] + "' which does not appear in this class.");
}
}
for (unsigned i=0; i<rParameterNames.size(); ++i)
{
SetVectorComponent(mParameters,index_map[i],rParameters[i]);
}
// Paranoia check
assert(GetVectorSize(mParameters) == rGetParameterNames().size());
}
Encoding* Encoding::create_bootstrap(STATE, const char* name,
Index index, OnigEncodingType* enc)
{
Encoding* e = create(state, enc);
symbol_map(state)->store(state, state->symbol(name), e);
index_map(state)->store(state, Fixnum::from(index), e);
add_constant(state, name, e);
return e;
}
bool CostMapCalculation::computeWindowIndices(ros::Time time,double update_radius)
{
int update_radius_map;
Eigen::Vector2f index_world, index_map;
// window update region
// only update cells within the max a curtain radius of current robot position
tf::StampedTransform transform;
// get local map transform
try
{
listener.waitForTransform(map_frame_id, local_transform_frame_id, time, ros::Duration(5));
listener.lookupTransform(map_frame_id, local_transform_frame_id, time, transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
return false;
}
index_world(0) = transform.getOrigin().x();
index_world(1) = transform.getOrigin().y();
index_map = world_map_transform.getC2Coords(index_world);
update_radius_map = floor(((double)update_radius/(double)cost_map.info.resolution));
// compute window min/max index
if(index_map(1) < update_radius_map)
min_index(1) = 0;
else
min_index(1) = index_map(1) - update_radius_map;
if(index_map(1) + update_radius_map > cost_map.info.height)
max_index(1) = cost_map.info.height;
else
max_index(1) = index_map(1) + update_radius_map;
if(index_map(0) < update_radius_map)
min_index(0) = 0;
else
min_index(0) = index_map(0) - update_radius_map;
if(index_map(0) + update_radius_map > cost_map.info.width)
max_index(0) = cost_map.info.width;
else
max_index(0) = index_map(0) + update_radius_map;
return true;
}
int main ()
{
Arrangement_2 arr;
// Construct an arrangement of seven intersecting line segments.
insert (arr, Segment_2 (Point_2 (1, 1), Point_2 (7, 1)));
insert (arr, Segment_2 (Point_2 (1, 1), Point_2 (3, 7)));
insert (arr, Segment_2 (Point_2 (1, 4), Point_2 (7, 1)));
insert (arr, Segment_2 (Point_2 (2, 2), Point_2 (9, 3)));
insert (arr, Segment_2 (Point_2 (2, 2), Point_2 (4, 4)));
insert (arr, Segment_2 (Point_2 (7, 1), Point_2 (9, 3)));
insert (arr, Segment_2 (Point_2 (3, 7), Point_2 (9, 3)));
// Create a mapping of the arrangement faces to indices.
CGAL::Arr_face_index_map<Arrangement_2> index_map (arr);
// Perform breadth-first search from the unbounded face, and use the BFS
// visitor to associate each arrangement face with its discover time.
Discover_time_bfs_visitor<CGAL::Arr_face_index_map<Arrangement_2> >
bfs_visitor (index_map);
Arrangement_2::Face_handle uf = arr.unbounded_face();
boost::breadth_first_search (Dual_arrangement_2 (arr), uf,
boost::vertex_index_map (index_map).
visitor (bfs_visitor));
// Print the results:
Arrangement_2::Face_iterator fit;
for (fit = arr.faces_begin(); fit != arr.faces_end(); ++fit)
{
std::cout << "Discover time " << fit->data() << " for ";
if (fit != uf)
{
std::cout << "face ";
print_ccb<Arrangement_2> (fit->outer_ccb());
}
else
std::cout << "the unbounded face." << std::endl;
}
return (0);
}
int main()
{
Arrangement_2 arr;
// Construct an arrangement of seven intersecting line segments.
// We keep a handle for the vertex v_0 that corresponds to the point (1,1).
Arrangement_2::Halfedge_handle e =
insert_non_intersecting_curve (arr, Segment_2 (Point_2 (1, 1),
Point_2 (7, 1)));
Arrangement_2::Vertex_handle v0 = e->source();
insert (arr, Segment_2 (Point_2 (1, 1), Point_2 (3, 7)));
insert (arr, Segment_2 (Point_2 (1, 4), Point_2 (7, 1)));
insert (arr, Segment_2 (Point_2 (2, 2), Point_2 (9, 3)));
insert (arr, Segment_2 (Point_2 (2, 2), Point_2 (4, 4)));
insert (arr, Segment_2 (Point_2 (7, 1), Point_2 (9, 3)));
insert (arr, Segment_2 (Point_2 (3, 7), Point_2 (9, 3)));
// Create a mapping of the arrangement vertices to indices.
Arr_vertex_index_map index_map(arr);
// Perform Dijkstra's algorithm from the vertex v0.
Edge_length_func edge_length;
boost::vector_property_map<double, Arr_vertex_index_map> dist_map(static_cast<unsigned int>(arr.number_of_vertices()), index_map);
boost::dijkstra_shortest_paths(arr, v0,
boost::vertex_index_map(index_map).
weight_map(edge_length).
distance_map(dist_map));
// Print the results:
Arrangement_2::Vertex_iterator vit;
std::cout << "The distances of the arrangement vertices from ("
<< v0->point() << ") :" << std::endl;
for (vit = arr.vertices_begin(); vit != arr.vertices_end(); ++vit)
std::cout << "(" << vit->point() << ") at distance "
<< dist_map[vit] << std::endl;
return 0;
}
void CostMapCalculation::callbackPointCloud(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sensor (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_base_link (new pcl::PointCloud<pcl::PointXYZ>);
ROS_DEBUG("HectorCM: received new point cloud");
// converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
pcl::fromROSMsg(*cloud_msg, *cloud_sensor);
// transform cloud to /map frame
try
{
listener.waitForTransform("base_stabilized", cloud_msg->header.frame_id,cloud_msg->header.stamp,ros::Duration(1.0));
pcl_ros::transformPointCloud("base_stabilized",*cloud_sensor,*cloud_base_link,listener);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ROS_DEBUG("HectorCM: pointcloud transform failed");
return;
}
// compute region of intereset
if(!computeWindowIndices(cloud_msg->header.stamp, update_radius_world))
return;
Eigen::Vector2f world_coords;
// define a cube with two points in space:
Eigen::Vector4f minPoint;
world_coords = world_map_transform.getC1Coords(min_index.cast<float>());
minPoint[0]=world_coords(0); // define minimum point x
minPoint[1]=world_coords(1); // define minimum point y
minPoint[2]=slize_min_height; // define minimum point z
Eigen::Vector4f maxPoint;
world_coords = world_map_transform.getC1Coords(max_index.cast<float>());
maxPoint[0]=world_coords(0); // define max point x
maxPoint[1]=world_coords(1); // define max point y
maxPoint[2]=slize_max_height; // define max point z
pcl::CropBox<pcl::PointXYZ> cropFilter;
cropFilter.setInputCloud (cloud_base_link);
cropFilter.setMin(minPoint);
cropFilter.setMax(maxPoint);
cropFilter.filter (*sliced_cloud);
ROS_DEBUG("HectorCM: slice size: %d", (int)sliced_cloud->size());
pub_cloud_slice.publish(sliced_cloud);
cloud_cost_map.data.assign(cost_map.info.width * cost_map.info.height,UNKNOWN_CELL);
// iterate trough all points
for (unsigned int k = 0; k < sliced_cloud->size(); ++k)
{
const pcl::PointXYZ& pt_cloud = sliced_cloud->points[k];
// allign grid points
Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));
cloud_cost_map.data[MAP_IDX(cost_map.info.width, (int)round(index_map(0)), (int)round(index_map(1)))] = OCCUPIED_CELL;
}
// set point cloud received flag
received_point_cloud = true;
// calculate cost map
if(received_grid_map)
{
if(received_elevation_map)
{
calculateCostMap(USE_GRID_AND_ELEVATION_MAP_AND_CLOUD_MAP);
}
else
{
calculateCostMap(USE_GRID_AND_CLOUD_MAP);
}
}
}
void ElevationVisualization::visualize_map(const hector_elevation_msgs::ElevationGrid& elevation_map, tf::StampedTransform local_map_transform)
{
int current_height_level;
for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
{
map_marker_array_msg.markers[i].points.clear();
}
map_marker_array_msg.markers.clear();
// each array stores all cubes of one height level:
map_marker_array_msg.markers.resize(max_height_levels+1);
for (int index_y = 0; index_y < (int)elevation_map.info.height; ++index_y)
{
for (int index_x = 0; index_x < (int)elevation_map.info.width; ++index_x)
{
// visualize only known cells
if(elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)] != (int16_t)-elevation_map.info.zero_elevation)
{
geometry_msgs::Point cube_center;
Eigen::Vector2f index_map(index_x, index_y);
Eigen::Vector2f index_world = world_map_transform.getC1Coords(index_map);
cube_center.x = index_world(0);//+elevation_map.info.resolution_xy/2.0;
cube_center.y = index_world(1);//+elevation_map.info.resolution_xy/2.0;
cube_center.z = (elevation_map.data[MAP_IDX(elevation_map.info.width, index_x, index_y)]-elevation_map.info.zero_elevation)*elevation_map.info.resolution_z;
current_height_level = max_height_levels/2+(int)round(std::min(std::max((double)cube_center.z+local_map_transform.getOrigin().z(), -(double)max_height), (double)max_height)*(double)max_height_levels/((double)max_height*2.0f));
map_marker_array_msg.markers[current_height_level].points.push_back(cube_center);
if(use_color_map)
{
double h = (1.0 - std::min(std::max((cube_center.z-min_height)/ (max_height - min_height), 0.0), 1.0)) *color_factor;
map_marker_array_msg.markers[current_height_level].colors.push_back(heightMapColor(h));
}
}
}
}
for (unsigned i = 0; i < map_marker_array_msg.markers.size(); ++i)
{
std::stringstream ss;
ss <<"Level "<<i;
map_marker_array_msg.markers[i].ns = ss.str();
map_marker_array_msg.markers[i].id = i;
map_marker_array_msg.markers[i].header.frame_id = "/map";
map_marker_array_msg.markers[i].header.stamp = elevation_map.header.stamp;
map_marker_array_msg.markers[i].lifetime = ros::Duration();
map_marker_array_msg.markers[i].type = visualization_msgs::Marker::CUBE_LIST;
map_marker_array_msg.markers[i].scale.x = elevation_map.info.resolution_xy;
map_marker_array_msg.markers[i].scale.y = elevation_map.info.resolution_xy;
map_marker_array_msg.markers[i].scale.z = elevation_map.info.resolution_z;
map_marker_array_msg.markers[i].color = marker_color;
if (map_marker_array_msg.markers[i].points.size() > 0)
map_marker_array_msg.markers[i].action = visualization_msgs::Marker::ADD;
else
map_marker_array_msg.markers[i].action = visualization_msgs::Marker::DELETE;
}
}
void ElevationMapping::cloudCallback(const sensor_msgs::PointCloud2ConstPtr& pointcloud2_sensor_msg)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud2_map_pcl (new pcl::PointCloud<pcl::PointXYZ> ()),
pointcloud2_sensor_pcl (new pcl::PointCloud<pcl::PointXYZ> ());
tf::StampedTransform local_map_transform;
ROS_DEBUG("HectorEM received a point cloud.");
// converting PointCloud2 msg format to pcl pointcloud format in order to read the 3d data
pcl::fromROSMsg(*pointcloud2_sensor_msg, *pointcloud2_sensor_pcl);
// transform cloud to /map frame
try
{
listener.waitForTransform(map_frame_id, pointcloud2_sensor_msg->header.frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
pcl_ros::transformPointCloud(map_frame_id,*pointcloud2_sensor_pcl,*pointcloud2_map_pcl,listener);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ROS_DEBUG("HectorEM: pointcloud transform failed");
return;
}
// get local map transform
try
{
listener.waitForTransform(map_frame_id, local_map_frame_id,pointcloud2_sensor_msg->header.stamp,ros::Duration(1.0));
listener.lookupTransform(map_frame_id, local_map_frame_id, pointcloud2_sensor_msg->header.stamp, local_map_transform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
ROS_DEBUG("HectorEM: localmap transform in cloud callback failed");
return;
}
bool local_map_is_subscribed = (pub_local_map.getNumSubscribers () > 0);
bool global_map_is_subscribed = (pub_global_map.getNumSubscribers () > 0);
if(local_map_is_subscribed)
local_elevation_map.data = std::vector<int16_t>(elevation_map_meta.width * elevation_map_meta.height,(int16_t)-elevation_map_meta.zero_elevation);
unsigned int size = (unsigned int)pointcloud2_map_pcl->points.size();
// iterate trough all points
for (unsigned int k = 0; k < size; ++k)
{
const pcl::PointXYZ& pt_cloud = pointcloud2_map_pcl->points[k];
double measurement_distance = pointcloud2_sensor_pcl->points[k].z;
// check for invalid measurements
if (isnan(pt_cloud.x) || isnan(pt_cloud.y) || isnan(pt_cloud.z))
continue;
// check max distance (manhatten norm)
if(max_observable_distance < measurement_distance)
continue;
// check min/max height
if(elevation_map_meta.min_elevation+local_map_transform.getOrigin().z() > pt_cloud.z || elevation_map_meta.max_elevation+local_map_transform.getOrigin().z() < pt_cloud.z)
continue;
// allign grid points
Eigen::Vector2f index_world(pt_cloud.x, pt_cloud.y);
Eigen::Vector2f index_map (world_map_transform.getC2Coords(index_world));
unsigned int cell_index = MAP_IDX(elevation_map_meta.width, (int)round(index_map(0)), (int)round(index_map(1)));
int16_t* pt_local_map = &local_elevation_map.data[cell_index];
int16_t* pt_global_map = &global_elevation_map.data[cell_index];
double* pt_var = &cell_variance[cell_index];
if(local_map_is_subscribed)
{
// elevation in current cell in meter
double cell_elevation = elevation_map_meta.resolution_z*(*pt_local_map-elevation_map_meta.zero_elevation);
// store maximum of each cell
if(pt_cloud.z > cell_elevation)
*pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
// filter each cell localy
// double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);
// if(*pt_local_map == (int16_t)-elevation_map_meta.zero_elevation)
// {
// // unknown cell -> use current measurement
// *pt_local_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
// *pt_var = measurement_variance;
// }
// else
// {
// // fuse cell_elevation with measurement
// *pt_local_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
// *pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
// }
}
if(publish_poseupdate || global_map_is_subscribed)
{
// fuse new measurements with existing map
// elevation in current cell in meter
double cell_elevation = elevation_map_meta.resolution_z*(*pt_global_map-elevation_map_meta.zero_elevation);
// measurement variance
double measurement_variance = sensor_variance*(measurement_distance*measurement_distance);
// mahalanobis distance
double mahalanobis_distance = sqrt((pt_cloud.z - cell_elevation)*(pt_cloud.z - cell_elevation)/(measurement_variance*measurement_variance));
if(pt_cloud.z > cell_elevation && (mahalanobis_distance > 5.0))
{
*pt_global_map = (int16_t)(round(pt_cloud.z/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
*pt_var = measurement_variance;
continue;
}
if((pt_cloud.z < cell_elevation) && (mahalanobis_distance > 5.0))
{
*pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
//*pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
*pt_var = measurement_variance;
continue;
}
*pt_global_map = (int16_t) (round(((measurement_variance * cell_elevation + *pt_var * pt_cloud.z)/(*pt_var + measurement_variance))/elevation_map_meta.resolution_z) + (int16_t)elevation_map_meta.zero_elevation);
*pt_var = (measurement_variance * *pt_var)/(measurement_variance + *pt_var);
}
}
if(local_map_is_subscribed)
{
// set the header information on the map
local_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
local_elevation_map.header.frame_id = map_frame_id;
pub_local_map.publish(local_elevation_map);
}
if(global_map_is_subscribed)
{
// set the header information on the map
global_elevation_map.header.stamp = pointcloud2_sensor_msg->header.stamp;
global_elevation_map.header.frame_id = map_frame_id;
pub_global_map.publish(global_elevation_map);
}
}
void ElevationMapping::timerCallback(const ros::TimerEvent& event)
{
tf::StampedTransform local_map_transform;
// get local map transform
try
{
listener.waitForTransform(map_frame_id, local_map_frame_id,ros::Time(0),ros::Duration(1.0));
listener.lookupTransform(map_frame_id, local_map_frame_id,ros::Time(0), local_map_transform);
}
catch (tf::TransformException ex)
{
ROS_DEBUG("HectorEM: localmap transform in timer callback failed");
ROS_ERROR("%s",ex.what());
return;
}
// allign grid points
Eigen::Vector2f index_world(local_map_transform.getOrigin().x(), local_map_transform.getOrigin().y());
Eigen::Vector2f index_map = world_map_transform.getC2Coords(index_world);
// check if elevation of current robot position is known, otherwise cancel pose update
if(global_elevation_map.data[MAP_IDX(elevation_map_meta.width, (int)index_map(0), (int)index_map(1))] != (int16_t)-elevation_map_meta.zero_elevation)
{
geometry_msgs::PoseWithCovarianceStamped cell_height_average;
int error_level = 0;
int pattern_cell_quantity = 4*poseupdate_used_pattern_size*poseupdate_used_pattern_size;
/// todo check if min/max index is inside map
// include neighbours
for(int x=index_map(0)-poseupdate_used_pattern_size;x<index_map(0)+poseupdate_used_pattern_size;x++)
{
for(int y=index_map(1)-poseupdate_used_pattern_size;y<index_map(1)+poseupdate_used_pattern_size;y++)
{
int cell_index = MAP_IDX(elevation_map_meta.width, x, y);
if(global_elevation_map.data[cell_index] == (int16_t)-elevation_map_meta.zero_elevation)
{
// unknown cell
error_level++;
}
else
{
// cell is knwon, accumulate cell hight
cell_height_average.pose.pose.position.z += (global_elevation_map.data[cell_index]-elevation_map_meta.zero_elevation)*elevation_map_meta.resolution_z;
}
}
}
// only publish pose update, if more than 1/2 of pattern cells are known
if(error_level < pattern_cell_quantity/2)
{
pattern_cell_quantity -= error_level;
cell_height_average.pose.pose.position.z = cell_height_average.pose.pose.position.z/(double)pattern_cell_quantity;
cell_height_average.header.frame_id = map_frame_id;
cell_height_average.header.stamp = ros::Time::now();
cell_height_average.pose.covariance.at(0) = 0.0; //no x-position information
cell_height_average.pose.covariance.at(7) = 0.0; //no y-position information
cell_height_average.pose.covariance.at(14) = 1.0/poseupdate_height_covariance;
pub_height_update.publish(cell_height_average);
ROS_DEBUG("HectorEM: published height update %f",cell_height_average.pose.pose.position.z);
}
}
}
Encoding* Encoding::utf8_encoding(STATE) {
return as<Encoding>(index_map(state)->fetch(state, Fixnum::from(eUtf8)));
}
Encoding* Encoding::ascii_encoding(STATE) {
return as<Encoding>(index_map(state)->fetch(state, Fixnum::from(eAscii)));
}
//plasticity integration routine
void J2Plasticity :: plastic_integrator( )
{
const double tolerance = (1.0e-8)*sigma_0 ;
const double dt = ops_Dt ; //time step
static Matrix dev_strain(3,3) ; //deviatoric strain
static Matrix dev_stress(3,3) ; //deviatoric stress
static Matrix normal(3,3) ; //normal to yield surface
double NbunN ; //normal bun normal
double norm_tau = 0.0 ; //norm of deviatoric stress
double inv_norm_tau = 0.0 ;
double phi = 0.0 ; //trial value of yield function
double trace = 0.0 ; //trace of strain
double gamma = 0.0 ; //consistency parameter
double resid = 1.0 ;
double tang = 0.0 ;
double theta = 0.0 ;
double theta_inv = 0.0 ;
double c1 = 0.0 ;
double c2 = 0.0 ;
double c3 = 0.0 ;
int i,j,k,l;
int ii, jj ;
int iteration_counter ;
const int max_iterations = 25 ;
//compute the deviatoric strains
trace = strain(0,0) + strain(1,1) + strain(2,2) ;
dev_strain = strain ;
for ( i = 0; i < 3; i++ )
dev_strain(i,i) -= ( one3*trace ) ;
//compute the trial deviatoric stresses
// dev_stress = (2.0*shear) * ( dev_strain - epsilon_p_n ) ;
dev_stress = dev_strain;
dev_stress -= epsilon_p_n;
dev_stress *= 2.0 * shear;
//compute norm of deviatoric stress
norm_tau = 0.0 ;
for ( i = 0; i < 3; i++ ){
for ( j = 0; j < 3; j++ )
norm_tau += dev_stress(i,j)*dev_stress(i,j) ;
} //end for i
norm_tau = sqrt( norm_tau ) ;
if ( norm_tau > tolerance ) {
inv_norm_tau = 1.0 / norm_tau ;
normal = inv_norm_tau * dev_stress ;
}
else {
normal.Zero( ) ;
inv_norm_tau = 0.0 ;
} //end if
//compute trial value of yield function
phi = norm_tau - root23 * q(xi_n) ;
// check if phi > 0
if ( phi > 0.0 ) { //plastic
//solve for gamma
gamma = 0.0 ;
resid = 1.0 ;
iteration_counter = 0 ;
while ( fabs(resid) > tolerance ) {
resid = norm_tau
- (2.0*shear) * gamma
- root23 * q( xi_n + root23*gamma )
- (eta/dt) * gamma ;
tang = - (2.0*shear)
- two3 * qprime( xi_n + root23*gamma )
- (eta/dt) ;
gamma -= ( resid / tang ) ;
iteration_counter++ ;
if ( iteration_counter > max_iterations ) {
opserr << "More than " << max_iterations ;
opserr << " iterations in constituive subroutine J2-plasticity \n" ;
break ;
} //end if
} //end while resid
gamma *= (1.0 - 1e-08) ;
//update plastic internal variables
epsilon_p_nplus1 = epsilon_p_n + gamma*normal ;
xi_nplus1 = xi_n + root23*gamma ;
//recompute deviatoric stresses
dev_stress = (2.0*shear) * ( dev_strain - epsilon_p_nplus1 ) ;
//compute the terms for plastic part of tangent
theta = (2.0*shear)
+ two3 * qprime( xi_nplus1 )
+ (eta/dt) ;
theta_inv = 1.0/theta ;
}
else { //elastic
//update history variables -- they remain unchanged
epsilon_p_nplus1 = epsilon_p_n ;
xi_nplus1 = xi_n ;
//no extra tangent terms to compute
gamma = 0.0 ;
theta = 0.0 ;
theta_inv = 0.0 ;
} //end if phi > 0
//add on bulk part of stress
stress = dev_stress ;
for ( i = 0; i < 3; i++ )
stress(i,i) += bulk*trace ;
//compute the tangent
c1 = -4.0 * shear * shear ;
c2 = c1 * theta_inv ;
c3 = c1 * gamma * inv_norm_tau ;
for ( ii = 0; ii < 6; ii++ ) {
for ( jj = 0; jj < 6; jj++ ) {
index_map( ii, i, j ) ;
index_map( jj, k, l ) ;
NbunN = normal(i,j)*normal(k,l) ;
//elastic terms
tangent[i][j][k][l] = bulk * IbunI[i][j][k][l] ;
tangent[i][j][k][l] += (2.0*shear) * IIdev[i][j][k][l] ;
//plastic terms
tangent[i][j][k][l] += c2 * NbunN ;
tangent[i][j][k][l] += c3 * ( IIdev[i][j][k][l] - NbunN ) ;
//minor symmetries
tangent [j][i][k][l] = tangent[i][j][k][l] ;
tangent [i][j][l][k] = tangent[i][j][k][l] ;
tangent [j][i][l][k] = tangent[i][j][k][l] ;
} // end for jj
} // end for ii
return ;
}
