void Mass::f(Eigen::Vector4f *pos, Eigen::Vector4f *result){
float stiff = 10.0f;
Eigen::Vector4f a;
a << 0,0,0,0;
Eigen::Vector4f v;
v << velocity.x(),velocity.y(),velocity.z(),0;
for(int i = 0; i<links.size(); i++){
Mass *link = links[i];
float length = (*pos-link->position).norm();
float stretch = length-2.0f/40;
Eigen::Vector4f force;
force = (stiff*(link->position-(*pos))*stretch);
if(force.norm()>.2){
force = force*(.2/force.norm());
}
a += force;
}
a.z() += .2f*GRAVITY*.0001f;
for(int i = 0; i<objects.size(); i++){
Eigen::Vector4f normal;
if(objects[i]->intersects(&position,&normal))
{
v *= 0;
a *= 0;
}
}
v += a;
*result = v;
}
bool
pcl::computePairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1,
const Eigen::Vector4f &p2, const Eigen::Vector4f &n2,
float &f1, float &f2, float &f3, float &f4)
{
Eigen::Vector4f dp2p1 = p2 - p1;
dp2p1[3] = 0.0f;
f4 = dp2p1.norm ();
if (f4 == 0.0f)
{
PCL_DEBUG ("[pcl::computePairFeatures] Euclidean distance between points is 0!\n");
f1 = f2 = f3 = f4 = 0.0f;
return (false);
}
Eigen::Vector4f n1_copy = n1,
n2_copy = n2;
n1_copy[3] = n2_copy[3] = 0.0f;
float angle1 = n1_copy.dot (dp2p1) / f4;
// Make sure the same point is selected as 1 and 2 for each pair
float angle2 = n2_copy.dot (dp2p1) / f4;
if (acos (fabs (angle1)) > acos (fabs (angle2)))
{
// switch p1 and p2
n1_copy = n2;
n2_copy = n1;
n1_copy[3] = n2_copy[3] = 0.0f;
dp2p1 *= (-1);
f3 = -angle2;
}
else
f3 = angle1;
// Create a Darboux frame coordinate system u-v-w
// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
Eigen::Vector4f v = dp2p1.cross3 (n1_copy);
v[3] = 0.0f;
float v_norm = v.norm ();
if (v_norm == 0.0f)
{
PCL_DEBUG ("[pcl::computePairFeatures] Norm of Delta x U is 0!\n");
f1 = f2 = f3 = f4 = 0.0f;
return (false);
}
// Normalize v
v /= v_norm;
Eigen::Vector4f w = n1_copy.cross3 (v);
// Do not have to normalize w - it is a unit vector by construction
v[3] = 0.0f;
f2 = v.dot (n2_copy);
w[3] = 0.0f;
// Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
f1 = atan2f (w.dot (n2_copy), n1_copy.dot (n2_copy)); // @todo optimize this
return (true);
}
bool
pcl::computeRGBPairFeatures (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1, const Eigen::Vector4i &colors1,
const Eigen::Vector4f &p2, const Eigen::Vector4f &n2, const Eigen::Vector4i &colors2,
float &f1, float &f2, float &f3, float &f4, float &f5, float &f6, float &f7)
{
Eigen::Vector4f dp2p1 = p2 - p1;
dp2p1[3] = 0.0f;
f4 = dp2p1.norm ();
if (f4 == 0.0f)
{
PCL_DEBUG ("Euclidean distance between points is 0!\n");
f1 = f2 = f3 = f4 = 0.0f;
return (false);
}
Eigen::Vector4f n1_copy = n1,
n2_copy = n2;
n1_copy[3] = n2_copy[3] = 0.0f;
float angle1 = n1_copy.dot (dp2p1) / f4;
f3 = angle1;
// Create a Darboux frame coordinate system u-v-w
// u = n1; v = (p_idx - q_idx) x u / || (p_idx - q_idx) x u ||; w = u x v
Eigen::Vector4f v = dp2p1.cross3 (n1_copy);
v[3] = 0.0f;
float v_norm = v.norm ();
if (v_norm == 0.0f)
{
PCL_DEBUG ("Norm of Delta x U is 0!\n");
f1 = f2 = f3 = f4 = 0.0f;
return (false);
}
// Normalize v
v /= v_norm;
Eigen::Vector4f w = n1_copy.cross3 (v);
// Do not have to normalize w - it is a unit vector by construction
v[3] = 0.0f;
f2 = v.dot (n2_copy);
w[3] = 0.0f;
// Compute f1 = arctan (w * n2, u * n2) i.e. angle of n2 in the x=u, y=w coordinate system
f1 = atan2f (w.dot (n2_copy), n1_copy.dot (n2_copy)); // @todo optimize this
// everything before was standard 4D-Darboux frame feature pair
// now, for the experimental color stuff
f5 = (colors2[0] != 0) ? static_cast<float> (colors1[0] / colors2[0]) : 1.0f;
f6 = (colors2[1] != 0) ? static_cast<float> (colors1[1] / colors2[1]) : 1.0f;
f7 = (colors2[2] != 0) ? static_cast<float> (colors1[2] / colors2[2]) : 1.0f;
// make sure the ratios are in the [-1, 1] interval
if (f5 > 1.0f) f5 = - 1.0f / f5;
if (f6 > 1.0f) f6 = - 1.0f / f6;
if (f7 > 1.0f) f7 = - 1.0f / f7;
return (true);
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::selectWithinDistance (
const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
{
// Check if the model is valid given the user constraints
if (!isModelValid (model_coefficients))
{
inliers.clear ();
return;
}
int nr_p = 0;
inliers.resize (indices_->size ());
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < indices_->size (); ++i)
{
Eigen::Vector4f pt (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
// Calculate the direction of the point from center
Eigen::Vector4f pp_pt_dir = pt - pt_proj;
pp_pt_dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan(opening_angle) * height.norm ();
height.normalize ();
// Calculate the cones perfect normals
Eigen::Vector4f cone_normal = sinf (opening_angle) * height + cosf (opening_angle) * pp_pt_dir;
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
double d_euclid = fabs (pointToAxisDistance (pt, model_coefficients) - actual_cone_radius);
// Calculate the angular distance between the point normal and the (dir=pt_proj->pt) vector
double d_normal = fabs (getAngle3D (n, cone_normal));
d_normal = (std::min) (d_normal, M_PI - d_normal);
if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
{
// Returns the indices of the points whose distances are smaller than the threshold
inliers[nr_p] = (*indices_)[i];
nr_p++;
}
}
inliers.resize (nr_p);
}
void PlaneSupportedCuboidEstimator::cloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
NODELET_INFO("cloudCallback");
if (!latest_polygon_msg_ || !latest_coefficients_msg_) {
JSK_NODELET_WARN("Not yet polygon is available");
return;
}
if (!tracker_) {
NODELET_INFO("initTracker");
// Update polygons_ vector
polygons_.clear();
for (size_t i = 0; i < latest_polygon_msg_->polygons.size(); i++) {
Polygon::Ptr polygon = Polygon::fromROSMsgPtr(latest_polygon_msg_->polygons[i].polygon);
polygons_.push_back(polygon);
}
// viewpoint
tf::StampedTransform transform
= lookupTransformWithDuration(tf_, sensor_frame_, msg->header.frame_id,
ros::Time(0.0),
ros::Duration(0.0));
tf::vectorTFToEigen(transform.getOrigin(), viewpoint_);
ParticleCloud::Ptr particles = initParticles();
tracker_.reset(new pcl::tracking::ROSCollaborativeParticleFilterTracker<pcl::PointXYZ, pcl::tracking::ParticleCuboid>);
tracker_->setCustomSampleFunc(boost::bind(&PlaneSupportedCuboidEstimator::sample, this, _1));
tracker_->setLikelihoodFunc(boost::bind(&PlaneSupportedCuboidEstimator::likelihood, this, _1, _2));
tracker_->setParticles(particles);
tracker_->setParticleNum(particle_num_);
support_plane_updated_ = false;
// Publish histograms
publishHistogram(particles, 0, pub_histogram_global_x_, msg->header);
publishHistogram(particles, 1, pub_histogram_global_y_, msg->header);
publishHistogram(particles, 2, pub_histogram_global_z_, msg->header);
publishHistogram(particles, 3, pub_histogram_global_roll_, msg->header);
publishHistogram(particles, 4, pub_histogram_global_pitch_, msg->header);
publishHistogram(particles, 5, pub_histogram_global_yaw_, msg->header);
publishHistogram(particles, 6, pub_histogram_dx_, msg->header);
publishHistogram(particles, 7, pub_histogram_dy_, msg->header);
publishHistogram(particles, 8, pub_histogram_dz_, msg->header);
// Publish particles
sensor_msgs::PointCloud2 ros_particles;
pcl::toROSMsg(*particles, ros_particles);
ros_particles.header = msg->header;
pub_particles_.publish(ros_particles);
}
else {
ParticleCloud::Ptr particles = tracker_->getParticles();
Eigen::Vector4f center;
pcl::compute3DCentroid(*particles, center);
if (center.norm() > fast_cloud_threshold_) {
estimate(msg);
}
}
}
void PlaneSupportedCuboidEstimator::fastCloudCallback(
const sensor_msgs::PointCloud2::ConstPtr& msg)
{
boost::mutex::scoped_lock lock(mutex_);
if (!tracker_) {
return;
}
ParticleCloud::Ptr particles = tracker_->getParticles();
Eigen::Vector4f center;
pcl::compute3DCentroid(*particles, center);
if (center.norm() < fast_cloud_threshold_) {
estimate(msg);
}
}
bool
loopDetection (int end, const CloudVector &clouds, double dist, int &first, int &last)
{
static double min_dist = -1;
int state = 0;
for (int i = end-1; i > 0; i--)
{
Eigen::Vector4f cstart, cend;
//TODO use pose of scan
pcl::compute3DCentroid (*(clouds[i].second), cstart);
pcl::compute3DCentroid (*(clouds[end].second), cend);
Eigen::Vector4f diff = cend - cstart;
double norm = diff.norm ();
//std::cout << "distance between " << i << " and " << end << " is " << norm << " state is " << state << std::endl;
if (state == 0 && norm > dist)
{
state = 1;
//std::cout << "state 1" << std::endl;
}
if (state > 0 && norm < dist)
{
state = 2;
//std::cout << "loop detected between scan " << i << " (" << clouds[i].first << ") and scan " << end << " (" << clouds[end].first << ")" << std::endl;
if (min_dist < 0 || norm < min_dist)
{
min_dist = norm;
first = i;
last = end;
}
}
}
//std::cout << "min_dist: " << min_dist << " state: " << state << " first: " << first << " end: " << end << std::endl;
if (min_dist > 0 && (state < 2 || end == int (clouds.size ()) - 1)) //TODO
{
min_dist = -1;
return true;
}
return false;
}
template <typename PointT, typename PointNT> bool
pcl::SampleConsensusModelCone<PointT, PointNT>::doSamplesVerifyModel (
const std::set<int> &indices, const Eigen::VectorXf &model_coefficients, const double threshold)
{
// Needs a valid model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::doSamplesVerifyModel] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return (false);
}
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float openning_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Iterate through the 3d points and calculate the distances from them to the cone
for (std::set<int>::const_iterator it = indices.begin (); it != indices.end (); ++it)
{
Eigen::Vector4f pt (input_->points[*it].x, input_->points[*it].y, input_->points[*it].z, 0);
// Calculate the point's projection on the cone axis
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
Eigen::Vector4f pt_proj = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pt_proj;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pt_proj;
double actual_cone_radius = tan (openning_angle) * height.norm ();
// Aproximate the distance from the point to the cone as the difference between
// dist(point,cone_axis) and actual cone radius
if (fabs (static_cast<double>(pointToAxisDistance (pt, model_coefficients) - actual_cone_radius)) > threshold)
return (false);
}
return (true);
}
bool
pcl::computePPFPairFeature (const Eigen::Vector4f &p1, const Eigen::Vector4f &n1,
const Eigen::Vector4f &p2, const Eigen::Vector4f &n2,
float &f1, float &f2, float &f3, float &f4)
{
Eigen::Vector4f delta = p2 - p1;
delta[3] = 0.0f;
// f4 = ||delta||
f4 = delta.norm ();
delta /= f4;
// f1 = n1 dot delta
f1 = n1[0] * delta[0] + n1[1] * delta[1] + n1[2] * delta[2];
// f2 = n2 dot delta
f2 = n2[0] * delta[0] + n2[1] * delta[1] + n2[2] * delta[2];
// f3 = n1 dot n2
f3 = n1[0] * n2[0] + n1[1] * n2[1] + n1[2] * n2[2];
return (true);
}
template <typename PointInT> double
pcl::tracking::NormalCoherence<PointInT>::computeCoherence (PointInT &source, PointInT &target)
{
Eigen::Vector4f n = source.getNormalVector4fMap ();
Eigen::Vector4f n_dash = target.getNormalVector4fMap ();
if ( n.norm () <= 1e-5 || n_dash.norm () <= 1e-5 )
{
PCL_ERROR("norm might be ZERO!\n");
std::cout << "source: " << source << std::endl;
std::cout << "target: " << target << std::endl;
exit (1);
return 0.0;
}
else
{
n.normalize ();
n_dash.normalize ();
double theta = pcl::getAngle3D (n, n_dash);
if (!pcl_isnan (theta))
return 1.0 / (1.0 + weight_ * theta * theta);
else
return 0.0;
}
}
int
main (int argc, char **argv)
{
double dist = 2.5;
pcl::console::parse_argument (argc, argv, "-d", dist);
int iter = 10;
pcl::console::parse_argument (argc, argv, "-i", iter);
int lumIter = 1;
pcl::console::parse_argument (argc, argv, "-l", lumIter);
double loopDist = 5.0;
pcl::console::parse_argument (argc, argv, "-D", loopDist);
int loopCount = 20;
pcl::console::parse_argument (argc, argv, "-c", loopCount);
pcl::registration::LUM<PointType> lum;
lum.setMaxIterations (lumIter);
lum.setConvergenceThreshold (0.001f);
std::vector<int> pcd_indices;
pcd_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
CloudVector clouds;
for (size_t i = 0; i < pcd_indices.size (); i++)
{
CloudPtr pc (new Cloud);
pcl::io::loadPCDFile (argv[pcd_indices[i]], *pc);
clouds.push_back (CloudPair (argv[pcd_indices[i]], pc));
std::cout << "loading file: " << argv[pcd_indices[i]] << " size: " << pc->size () << std::endl;
lum.addPointCloud (clouds[i].second);
}
for (int i = 0; i < iter; i++)
{
for (size_t i = 1; i < clouds.size (); i++)
for (size_t j = 0; j < i; j++)
{
Eigen::Vector4f ci, cj;
pcl::compute3DCentroid (*(clouds[i].second), ci);
pcl::compute3DCentroid (*(clouds[j].second), cj);
Eigen::Vector4f diff = ci - cj;
//std::cout << i << " " << j << " " << diff.norm () << std::endl;
if(diff.norm () < loopDist && (i - j == 1 || i - j > loopCount))
{
if(i - j > loopCount)
std::cout << "add connection between " << i << " (" << clouds[i].first << ") and " << j << " (" << clouds[j].first << ")" << std::endl;
pcl::registration::CorrespondenceEstimation<PointType, PointType> ce;
ce.setInputTarget (clouds[i].second);
ce.setInputSource (clouds[j].second);
pcl::CorrespondencesPtr corr (new pcl::Correspondences);
ce.determineCorrespondences (*corr, dist);
if (corr->size () > 2)
lum.setCorrespondences (j, i, corr);
}
}
lum.compute ();
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
//std::cout << i << ": " << lum.getTransformation (i) (0, 3) << " " << lum.getTransformation (i) (1, 3) << " " << lum.getTransformation (i) (2, 3) << std::endl;
clouds[i].second = lum.getTransformedCloud (i);
}
}
for(size_t i = 0; i < lum.getNumVertices (); i++)
{
std::string result_filename (clouds[i].first);
result_filename = result_filename.substr (result_filename.rfind ("/") + 1);
pcl::io::savePCDFileBinary (result_filename.c_str (), *(clouds[i].second));
//std::cout << "saving result to " << result_filename << std::endl;
}
return 0;
}
template<typename PointInT, typename PointNT, typename PointOutT> void
pcl::OURCVFHEstimation<PointInT, PointNT, PointOutT>::computeRFAndShapeDistribution (PointInTPtr & processed, PointCloudOut & output,
std::vector<pcl::PointIndices> & cluster_indices)
{
PointCloudOut ourcvfh_output;
for (size_t i = 0; i < centroids_dominant_orientations_.size (); i++)
{
std::vector < Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transformations;
PointInTPtr grid (new pcl::PointCloud<PointInT>);
sgurf (centroids_dominant_orientations_[i], dominant_normals_[i], processed, transformations, grid, cluster_indices[i]);
for (size_t t = 0; t < transformations.size (); t++)
{
pcl::transformPointCloud (*processed, *grid, transformations[t]);
transforms_.push_back (transformations[t]);
valid_transforms_.push_back (true);
std::vector < Eigen::VectorXf > quadrants (8);
int size_hists = 13;
int num_hists = 8;
for (int k = 0; k < num_hists; k++)
quadrants[k].setZero (size_hists);
Eigen::Vector4f centroid_p;
centroid_p.setZero ();
Eigen::Vector4f max_pt;
pcl::getMaxDistance (*grid, centroid_p, max_pt);
max_pt[3] = 0;
double distance_normalization_factor = (centroid_p - max_pt).norm ();
float hist_incr;
if (normalize_bins_)
hist_incr = 100.0f / static_cast<float> (grid->points.size () - 1);
else
hist_incr = 1.0f;
float * weights = new float[num_hists];
float sigma = 0.01f; //1cm
float sigma_sq = sigma * sigma;
for (int k = 0; k < static_cast<int> (grid->points.size ()); k++)
{
Eigen::Vector4f p = grid->points[k].getVector4fMap ();
p[3] = 0.f;
float d = p.norm ();
//compute weight for all octants
float wx = 1.f - std::exp (-((p[0] * p[0]) / (2.f * sigma_sq))); //how is the weight distributed among two semi-cubes
float wy = 1.f - std::exp (-((p[1] * p[1]) / (2.f * sigma_sq)));
float wz = 1.f - std::exp (-((p[2] * p[2]) / (2.f * sigma_sq)));
//distribute the weights using the x-coordinate
if (p[0] >= 0)
{
for (size_t ii = 0; ii <= 3; ii++)
weights[ii] = 0.5f - wx * 0.5f;
for (size_t ii = 4; ii <= 7; ii++)
weights[ii] = 0.5f + wx * 0.5f;
}
else
{
for (size_t ii = 0; ii <= 3; ii++)
weights[ii] = 0.5f + wx * 0.5f;
for (size_t ii = 4; ii <= 7; ii++)
weights[ii] = 0.5f - wx * 0.5f;
}
//distribute the weights using the y-coordinate
if (p[1] >= 0)
{
for (size_t ii = 0; ii <= 1; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
for (size_t ii = 4; ii <= 5; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
for (size_t ii = 2; ii <= 3; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
for (size_t ii = 6; ii <= 7; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
}
else
{
for (size_t ii = 0; ii <= 1; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
for (size_t ii = 4; ii <= 5; ii++)
weights[ii] *= 0.5f + wy * 0.5f;
for (size_t ii = 2; ii <= 3; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
for (size_t ii = 6; ii <= 7; ii++)
weights[ii] *= 0.5f - wy * 0.5f;
}
//distribute the weights using the z-coordinate
if (p[2] >= 0)
{
for (size_t ii = 0; ii <= 7; ii += 2)
weights[ii] *= 0.5f - wz * 0.5f;
for (size_t ii = 1; ii <= 7; ii += 2)
weights[ii] *= 0.5f + wz * 0.5f;
}
else
{
for (size_t ii = 0; ii <= 7; ii += 2)
weights[ii] *= 0.5f + wz * 0.5f;
for (size_t ii = 1; ii <= 7; ii += 2)
weights[ii] *= 0.5f - wz * 0.5f;
}
int h_index = static_cast<int> (std::floor (size_hists * (d / distance_normalization_factor)));
for (int j = 0; j < num_hists; j++)
quadrants[j][h_index] += hist_incr * weights[j];
}
//copy to the cvfh signature
PointCloudOut vfh_signature;
vfh_signature.points.resize (1);
vfh_signature.width = vfh_signature.height = 1;
for (int d = 0; d < 308; ++d)
vfh_signature.points[0].histogram[d] = output.points[i].histogram[d];
int pos = 45 * 3;
for (int k = 0; k < num_hists; k++)
{
for (int ii = 0; ii < size_hists; ii++, pos++)
{
vfh_signature.points[0].histogram[pos] = quadrants[k][ii];
}
}
ourcvfh_output.points.push_back (vfh_signature.points[0]);
delete[] weights;
}
}
output = ourcvfh_output;
}
template <typename PointT, typename PointNT> void
pcl::SampleConsensusModelCone<PointT, PointNT>::projectPoints (
const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields)
{
// Needs a valid set of model coefficients
if (model_coefficients.size () != 7)
{
PCL_ERROR ("[pcl::SampleConsensusModelCone::projectPoints] Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
return;
}
projected_points.header = input_->header;
projected_points.is_dense = input_->is_dense;
Eigen::Vector4f apex (model_coefficients[0], model_coefficients[1], model_coefficients[2], 0);
Eigen::Vector4f axis_dir (model_coefficients[3], model_coefficients[4], model_coefficients[5], 0);
float opening_angle = model_coefficients[6];
float apexdotdir = apex.dot (axis_dir);
float dirdotdir = 1.0f / axis_dir.dot (axis_dir);
// Copy all the data fields from the input cloud to the projected one?
if (copy_data_fields)
{
// Allocate enough space and copy the basics
projected_points.points.resize (input_->points.size ());
projected_points.width = input_->width;
projected_points.height = input_->height;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < projected_points.points.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[i], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < inliers.size (); ++i)
{
Eigen::Vector4f pt (input_->points[inliers[i]].x,
input_->points[inliers[i]].y,
input_->points[inliers[i]].z,
1);
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
pcl::Vector4fMap pp = projected_points.points[inliers[i]].getVector4fMap ();
pp.matrix () = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pp;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pp;
float actual_cone_radius = tanf (opening_angle) * height.norm ();
// Calculate the projection of the point onto the cone
pp += dir * actual_cone_radius;
}
}
else
{
// Allocate enough space and copy the basics
projected_points.points.resize (inliers.size ());
projected_points.width = static_cast<uint32_t> (inliers.size ());
projected_points.height = 1;
typedef typename pcl::traits::fieldList<PointT>::type FieldList;
// Iterate over each point
for (size_t i = 0; i < inliers.size (); ++i)
// Iterate over each dimension
pcl::for_each_type <FieldList> (NdConcatenateFunctor <PointT, PointT> (input_->points[inliers[i]], projected_points.points[i]));
// Iterate through the 3d points and calculate the distances from them to the cone
for (size_t i = 0; i < inliers.size (); ++i)
{
pcl::Vector4fMap pp = projected_points.points[i].getVector4fMap ();
pcl::Vector4fMapConst pt = input_->points[inliers[i]].getVector4fMap ();
float k = (pt.dot (axis_dir) - apexdotdir) * dirdotdir;
// Calculate the projection of the point on the line
pp.matrix () = apex + k * axis_dir;
Eigen::Vector4f dir = pt - pp;
dir.normalize ();
// Calculate the actual radius of the cone at the level of the projected point
Eigen::Vector4f height = apex - pp;
float actual_cone_radius = tanf (opening_angle) * height.norm ();
// Calculate the projection of the point onto the cone
pp += dir * actual_cone_radius;
}
}
}
/*
* Joint tracking based on their previous positions and velocities
*/
void
pcl::gpu::people::PeopleDetector::alphaBetaTracking ()
{
float outlier_thresh = 1.8; //Outliers above this threshold are dismissed (old value is used)
float outlier_thresh2 = 0.1; //Used for special handling of the hands. If the hands position change is higher then thresh2, tracking parameters are set higher (position change happens faster)
float outlier_thresh3 = 0.15; //for hand correction
for (int i = 0; i < num_parts_all; i++)
{
Eigen::Vector4f measured = skeleton_joints_[i];
Eigen::Vector4f prev = skeleton_joints_prev_[i];
Eigen::Vector4f velocity_prev = joints_velocity_[i];
bool valid = (measured - (prev)).norm () < outlier_thresh;
bool valid2 = (measured - (prev + velocity_prev)).norm () < outlier_thresh2;
//hand
//hand correction
//Left hand
if (i == Lhand && skeleton_joints_[Lforearm][0] != -1 && skeleton_joints_[Lelbow][0] != -1)
{
//Expected position based on forearm and elbow positions
Eigen::Vector4f dir = skeleton_joints_[Lforearm] - skeleton_joints_[Lelbow];
if (dir.norm () < 0.04)
dir *= 2.0;
Eigen::Vector4f corrected = skeleton_joints_[Lforearm] + dir;
//If the measured position is too far away from the expected position we use the expected position
if ( ( (corrected - measured).norm () > outlier_thresh3 && !valid2) || (corrected - measured).norm () > 0.3)
measured = corrected;
}
//Right hand (same as left hand)
if (i == Rhand && skeleton_joints_[Rforearm][0] != -1 && skeleton_joints_[Relbow][0] != -1)
{
Eigen::Vector4f dir = skeleton_joints_[Rforearm] - skeleton_joints_[Relbow];
if (dir.norm () < 0.04)
dir *= 2.0;
Eigen::Vector4f corrected = skeleton_joints_[Rforearm] + dir;
if ( ( (corrected - measured).norm () > outlier_thresh3 && !valid2) || (corrected - measured).norm () > 0.3)
measured = corrected;
}
valid = (measured - (prev)).norm () < outlier_thresh;
//going through all dimensions
for (int dim = 0; dim < 3; dim++)
{
float xm = measured[dim];
float xk = prev[dim] + (velocity_prev[dim] * dt_); //predicted position
float vk = velocity_prev[dim]; //velocity update
float rk = xm - xk; //difference between measured and predicted
//new position and velocity
if (prev[dim] == -1.0 || prev[dim] == 0.0) //if the old values are invalid, use the new ones
{
xk = measured[dim];
vk = 0.0;
}
else
{
if (valid && ! (xm == -1) && ! (xm == 0)) //Apply tracking
{
//If its the hands, arms and elbows and the joint has moved fast
//we increase the tracking parameters, so that the position changes faster
//the reason is that hand position might change much faster then the position of other joints
if (!valid2 && i != Lhand && i != Rhand && i != Lforearm && i != Rforearm && i != Lelbow && i != Relbow && i != Lfoot && i != Rfoot)
{
xk += alpha_tracking_ * rk * 0.2;
vk += (beta_tracking_ * rk * 0.5) / dt_;
}
else
{
if (i == Lhand || i == Rhand) //Increase the velocity update if it is a hand (because the hand position changes much faster)
{
xk += alpha_tracking_ * rk;
vk += (beta_tracking_ * 5.0 * rk) / dt_;
}
else
{ //If it's not a hand use usual tracking parameters
xk += alpha_tracking_ * rk;
vk += (beta_tracking_ * rk) / dt_;
}
}
}
else
{
xk = prev[dim];
vk = velocity_prev[dim];
}
}
skeleton_joints_[i][dim] = xk;
joints_velocity_[i][dim] = vk;
}
}
}