C++ Vector4f::norm方法代码示例

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
//.........这里部分代码省略.........

/*
 * 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
        {
//.........这里部分代码省略.........