const char* test_memory_iterator() {
gc_heap* heap = gc_make_heap(16 * GC_PAGE_SIZE, GC_POLICY_NONGEN, false);
gc_page_queue queue;
queue_even_pages(heap, &queue);
gc_memory_iterator itr;
begin_memory_iteration(heap, &queue, &itr, heap->heap_memory, 0);
do {
} while(advance_point(&itr, 32, true));
os_pointer expected_point
= heap->heap_memory + heap->heap_size - GC_PAGE_SIZE;
if(current_point(&itr) != expected_point)
return "advance 1 failed";
os_word expected_offset = 100;
begin_memory_iteration(heap, &queue, &itr, heap->heap_memory + 100, 0);
do {
os_pointer current_pointer = current_point(&itr);
os_word current_offset = current_pointer - heap->heap_memory;
if(current_offset != expected_offset)
return "advance 2 failed";
if(((expected_offset + 500) / GC_PAGE_SIZE)
!= (expected_offset / GC_PAGE_SIZE)) {
expected_offset += (GC_PAGE_SIZE - expected_offset % GC_PAGE_SIZE);
expected_offset += GC_PAGE_SIZE;
}
expected_offset += 500;
} while(advance_point(&itr, 500, true));
expected_offset = 100;
os_word increment = 500;
begin_memory_iteration(heap, &queue, &itr, heap->heap_memory, 0);
do {
increment = 500;
if(((expected_offset + 500) / GC_PAGE_SIZE)
!= (expected_offset / GC_PAGE_SIZE)) {
os_word remainder = GC_PAGE_SIZE - expected_offset % GC_PAGE_SIZE;
expected_offset += remainder;
increment -= remainder;
expected_offset += GC_PAGE_SIZE;
}
} while(advance_point(&itr, increment, false));
gc_destroy_heap(heap);
return "passed";
}
template <typename PointT> Eigen::VectorXf
open_ptrack::detection::GroundplaneEstimation<PointT>::computeFromTF (tf::Transform worldToCamTransform)
{
// Select 3 points in world reference frame:
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_points (new pcl::PointCloud<pcl::PointXYZ>);
ground_points->points.push_back(pcl::PointXYZ(0.0, 0.0, 0.0));
ground_points->points.push_back(pcl::PointXYZ(1.0, 0.0, 0.0));
ground_points->points.push_back(pcl::PointXYZ(0.0, 1.0, 0.0));
// Transform points to camera reference frame:
for (unsigned int i = 0; i < ground_points->points.size(); i++)
{
tf::Vector3 current_point(ground_points->points[i].x, ground_points->points[i].y, ground_points->points[i].z);
current_point = worldToCamTransform(current_point);
ground_points->points[i].x = current_point.x();
ground_points->points[i].y = current_point.y();
ground_points->points[i].z = current_point.z();
}
// Compute ground equation:
std::vector<int> ground_points_indices;
for (unsigned int i = 0; i < ground_points->points.size(); i++)
ground_points_indices.push_back(i);
pcl::SampleConsensusModelPlane<pcl::PointXYZ> model_plane(ground_points);
Eigen::VectorXf ground_coeffs;
model_plane.computeModelCoefficients(ground_points_indices,ground_coeffs);
std::cout << "Ground plane coefficients obtained from calibration: " << ground_coeffs(0) << ", " << ground_coeffs(1) << ", " << ground_coeffs(2) <<
", " << ground_coeffs(3) << "." << std::endl;
return ground_coeffs;
}
void
GapHeatTransfer::computeGapValues()
{
if (!_quadrature)
{
_has_info = true;
_gap_temp = _gap_temp_value[_qp];
_gap_distance = _gap_distance_value[_qp];
}
else
{
Node * qnode = _mesh.getQuadratureNode(_current_elem, _current_side, _qp);
PenetrationInfo * pinfo = _penetration_locator->_penetration_info[qnode->id()];
_gap_temp = 0.0;
_gap_distance = 88888;
_has_info = false;
_edge_multiplier = 1.0;
if (pinfo)
{
_gap_distance = pinfo->_distance;
_has_info = true;
Elem * slave_side = pinfo->_side;
std::vector<std::vector<Real> > & slave_side_phi = pinfo->_side_phi;
_gap_temp = _variable->getValue(slave_side, slave_side_phi);
Real tangential_tolerance = _penetration_locator->getTangentialTolerance();
if (tangential_tolerance != 0.0)
{
_edge_multiplier = 1.0 - pinfo->_tangential_distance / tangential_tolerance;
if (_edge_multiplier < 0.0)
{
_edge_multiplier = 0.0;
}
}
}
else
{
if (_warnings)
{
std::stringstream msg;
msg << "No gap value information found for node ";
msg << qnode->id();
msg << " on processor ";
msg << processor_id();
mooseWarning( msg.str() );
}
}
}
Point current_point(_q_point[_qp]);
GapConductance::computeGapRadii(_gap_geometry_type, current_point, _p1, _p2, _gap_distance, _normals[_qp], _r1, _r2, _radius);
}
void KMeans::run() {
int moves = 100;
int iterator = 0;
int nonempty = 0;
while (moves > 0 && iterator < __maxIter) {
moves = 0;
for (int i = 0; i < __k; ++i) {
for (int j = 0; j < __clusters[i]->getSize(); ++j)
{
Cluster &c = *(__clusters[i]);
Point current_point(__dimensionality);
current_point = c[j];
int smallest_dist_index = 0;
double smallest_dist = current_point.distanceTo(*__initCentroids[0]);
for (int e = 0; e < __k; e++) {
if (current_point.distanceTo(*__initCentroids[e]) < smallest_dist) {
smallest_dist = current_point.distanceTo(*__initCentroids[e]);
smallest_dist_index = e;
}
}
Cluster::Move change_clusters(current_point, *__clusters[i], *__clusters[smallest_dist_index]);
change_clusters.perform();
for (int c = 0; c < __k; ++c) {
__clusters[c]->centroid.compute();
}
if (*__clusters[i] != *__clusters[smallest_dist_index]) {
moves++;
}
}
}
iterator++;
}
Point inf(__dimensionality);
for (int i = 0; i < __dimensionality; ++i) {
inf[i] = std::numeric_limits<double>::max();
}
for (int i = 0; i < __k; ++i) {
if (__clusters[i]->centroid.get() != inf) {
++nonempty;
}
}
__numIter = iterator;
__numMovesLastIter = moves;
__numNonempty = nonempty;
}
void
GapConductance::computeGapValues()
{
if (!_quadrature)
{
_has_info = true;
_gap_temp = _gap_temp_value[_qp];
_gap_distance = _gap_distance_value[_qp];
}
else
{
Node * qnode = _mesh.getQuadratureNode(_current_elem, _current_side, _qp);
PenetrationInfo * pinfo = _penetration_locator->_penetration_info[qnode->id()];
_gap_temp = 0.0;
_gap_distance = 88888;
_has_info = false;
if (pinfo)
{
_gap_distance = pinfo->_distance;
_has_info = true;
const Elem * slave_side = pinfo->_side;
std::vector<std::vector<Real>> & slave_side_phi = pinfo->_side_phi;
std::vector<dof_id_type> slave_side_dof_indices;
_dof_map->dof_indices(slave_side, slave_side_dof_indices, _temp_var->number());
for (unsigned int i = 0; i < slave_side_dof_indices.size(); ++i)
{
// The zero index is because we only have one point that the phis are evaluated at
_gap_temp += slave_side_phi[i][0] * (*(*_serialized_solution))(slave_side_dof_indices[i]);
}
}
else
{
if (_warnings)
mooseWarning("No gap value information found for node ",
qnode->id(),
" on processor ",
processor_id(),
" at coordinate ",
Point(*qnode));
}
}
Point current_point(_q_point[_qp]);
computeGapRadii(
_gap_geometry_type, current_point, _p1, _p2, _gap_distance, _normals[_qp], _r1, _r2, _radius);
}
void parse_input(std::vector<std::shared_ptr<line_segment>>& polygon, std::ifstream& in)
{
std::string temp = "";
std::getline(in, temp);
int n = std::atoi(temp.c_str());
std::getline(in, temp);
std::shared_ptr<point> first_point(new point(temp));
std::shared_ptr<point> previous_point = first_point;
for (int i = 0; i < n - 1; ++i)
{
std::getline(in, temp);
std::shared_ptr<point> current_point(new point(temp));
polygon.push_back(std::shared_ptr<line_segment>(new line_segment(previous_point, current_point)));
previous_point = current_point;
}
polygon.push_back(std::shared_ptr<line_segment>(new line_segment(previous_point, first_point)));
}
template <typename PointT> Eigen::VectorXf
open_ptrack::detection::GroundplaneEstimation<PointT>::computeFromTF (std::string camera_frame, std::string ground_frame)
{
// Select 3 points in world reference frame:
pcl::PointCloud<pcl::PointXYZ>::Ptr ground_points (new pcl::PointCloud<pcl::PointXYZ>);
ground_points->points.push_back(pcl::PointXYZ(0.0, 0.0, 0.0));
ground_points->points.push_back(pcl::PointXYZ(1.0, 0.0, 0.0));
ground_points->points.push_back(pcl::PointXYZ(0.0, 1.0, 0.0));
// Read transform between world and camera reference frame:
tf::TransformListener tfListener;
tf::StampedTransform worldToCamTransform;
try
{
tfListener.waitForTransform(camera_frame, ground_frame, ros::Time(0), ros::Duration(3.0), ros::Duration(0.01));
tfListener.lookupTransform(camera_frame, ground_frame, ros::Time(0), worldToCamTransform);
}
catch (tf::TransformException ex)
{
ROS_ERROR("%s",ex.what());
}
// Transform points to camera reference frame:
for (unsigned int i = 0; i < ground_points->points.size(); i++)
{
tf::Vector3 current_point(ground_points->points[i].x, ground_points->points[i].y, ground_points->points[i].z);
current_point = worldToCamTransform(current_point);
ground_points->points[i].x = current_point.x();
ground_points->points[i].y = current_point.y();
ground_points->points[i].z = current_point.z();
}
// Compute ground equation:
std::vector<int> ground_points_indices;
for (unsigned int i = 0; i < ground_points->points.size(); i++)
ground_points_indices.push_back(i);
pcl::SampleConsensusModelPlane<pcl::PointXYZ> model_plane(ground_points);
Eigen::VectorXf ground_coeffs;
model_plane.computeModelCoefficients(ground_points_indices,ground_coeffs);
std::cout << "Ground plane coefficients obtained from calibration: " << ground_coeffs(0) << ", " << ground_coeffs(1) << ", " << ground_coeffs(2) <<
", " << ground_coeffs(3) << "." << std::endl;
return ground_coeffs;
}
