C++ MatrixXf::setZero方法代码示例

template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::vector<int> &spfh_hist_lookup,
    Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
{
  // Allocate enough space to hold the NN search results
  // \note This resize is irrelevant for a radiusSearch ().
  std::vector<int> nn_indices (k_);
  std::vector<float> nn_dists (k_);

  std::set<int> spfh_indices;
  spfh_hist_lookup.resize (surface_->points.size ());

  // Build a list of (unique) indices for which we will need to compute SPFH signatures
  // (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
  if (surface_ != input_ ||
      indices_->size () != surface_->points.size ())
  { 
    for (size_t idx = 0; idx < indices_->size (); ++idx)
    {
      int p_idx = (*indices_)[idx];
      if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
        continue;

      spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
    }
  }
  else
  {
    // Special case: When a feature must be computed at every point, there is no need for a neighborhood search
    for (size_t idx = 0; idx < indices_->size (); ++idx)
      spfh_indices.insert (static_cast<int> (idx));
  }

  // Initialize the arrays that will store the SPFH signatures
  size_t data_size = spfh_indices.size ();
  hist_f1.setZero (data_size, nr_bins_f1_);
  hist_f2.setZero (data_size, nr_bins_f2_);
  hist_f3.setZero (data_size, nr_bins_f3_);

  // Compute SPFH signatures for every point that needs them
  std::set<int>::iterator spfh_indices_itr = spfh_indices.begin ();
  for (int i = 0; i < static_cast<int> (spfh_indices.size ()); ++i)
  {
    // Get the next point index
    int p_idx = *spfh_indices_itr;
    ++spfh_indices_itr;

    // Find the neighborhood around p_idx
    if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
      continue;

    // Estimate the SPFH signature around p_idx
    computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1, hist_f2, hist_f3);

    // Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
    spfh_hist_lookup[p_idx] = i;
  }
}

	void addEigenMatrixRow( Eigen::MatrixXf &m )
	{
		Eigen::MatrixXf temp=m;
		m.resize(m.rows()+1,m.cols());
		m.setZero();
		m.block(0,0,temp.rows(),temp.cols())=temp;
	}

	void calcCovarWeighed(const float *input, const double *inputWeights, int vectorLength, int nVectors, Eigen::MatrixXf &out_covMat, const Eigen::VectorXf &mean)
	{
		//compute covariance matrix
		out_covMat.setZero();

		//Eigen::MatrixXf inMat = Eigen::MatrixXf::Zero(vectorLength,nVectors);
		//for (int i=0;i<vectorLength;i++){
		//	for(int k=0;k<nVectors;k++){
		//		inMat(i,k) = ((float)inputWeights[k])*(input[k*vectorLength+i]-mean[i]);
		//	}
		//}

		//out_covMat = (inMat*inMat.transpose())*(1.0/((float)nVectors-1.0));

		for (int i=0; i<vectorLength; i++){
			for (int j=0; j<vectorLength; j++){
				double avg=0;
				double iMean=mean[i];
				double jMean=mean[j];
				double wSum=0;
				for (int k=0; k<nVectors; k++){
					double w=inputWeights[k];
					avg+=w*(input[k*vectorLength+i]-iMean)*(input[k*vectorLength+j]-jMean);
					wSum+=w;
				}
				avg/=wSum;
				out_covMat(i,j)=(float)avg;
			}
		}
	}

template <typename PointInT, typename PointOutT> void
pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeIntensitySpinImage (
      const PointCloudIn &cloud, float radius, float sigma, 
      int k,
      const std::vector<int> &indices, 
      const std::vector<float> &squared_distances, 
      Eigen::MatrixXf &intensity_spin_image)
{
  // Determine the number of bins to use based on the size of intensity_spin_image
  int nr_distance_bins = static_cast<int> (intensity_spin_image.cols ());
  int nr_intensity_bins = static_cast<int> (intensity_spin_image.rows ());

  // Find the min and max intensity values in the given neighborhood
  float min_intensity = std::numeric_limits<float>::max ();
  float max_intensity = -std::numeric_limits<float>::max ();
  for (int idx = 0; idx < k; ++idx)
  {
    min_intensity = (std::min) (min_intensity, cloud.points[indices[idx]].intensity);
    max_intensity = (std::max) (max_intensity, cloud.points[indices[idx]].intensity);
  }

  float constant = 1.0f / (2.0f * sigma_ * sigma_);
  // Compute the intensity spin image
  intensity_spin_image.setZero ();
  for (int idx = 0; idx < k; ++idx)
  {
    // Normalize distance and intensity values to: 0.0 <= d,i < nr_distance_bins,nr_intensity_bins
    const float eps = std::numeric_limits<float>::epsilon ();
    float d = static_cast<float> (nr_distance_bins) * std::sqrt (squared_distances[idx]) / (radius + eps);
    float i = static_cast<float> (nr_intensity_bins) * 
              (cloud.points[indices[idx]].intensity - min_intensity) / (max_intensity - min_intensity + eps);

    if (sigma == 0)
    {
      // If sigma is zero, update the histogram with no smoothing kernel
      int d_idx = static_cast<int> (d);
      int i_idx = static_cast<int> (i);
      intensity_spin_image (i_idx, d_idx) += 1;
    }
    else
    {
      // Compute the bin indices that need to be updated (+/- 3 standard deviations)
      int d_idx_min = (std::max)(static_cast<int> (floor (d - 3*sigma)), 0);
      int d_idx_max = (std::min)(static_cast<int> (ceil  (d + 3*sigma)), nr_distance_bins - 1);
      int i_idx_min = (std::max)(static_cast<int> (floor (i - 3*sigma)), 0);
      int i_idx_max = (std::min)(static_cast<int> (ceil  (i + 3*sigma)), nr_intensity_bins - 1);
   
      // Update the appropriate bins of the histogram 
      for (int i_idx = i_idx_min; i_idx <= i_idx_max; ++i_idx)  
      {
        for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
        {
          // Compute a "soft" update weight based on the distance between the point and the bin
          float w = expf (-powf (d - static_cast<float> (d_idx), 2.0f) * constant - powf (i - static_cast<float> (i_idx), 2.0f) * constant);
          intensity_spin_image (i_idx, d_idx) += w;
        }
      }
    }
  }
}

void v_disparity_data_to_matrix(const FastGroundPlaneEstimator::v_disparity_data_t &image_data,
                                Eigen::MatrixXf &image_matrix)
{
    typedef FastGroundPlaneEstimator::v_disparity_data_t::const_reference row_slice_t;

    const int rows = image_data.shape()[0], cols = image_data.shape()[1];
    image_matrix.setZero(rows, cols);
    for(int row=0; row < rows; row +=1)
    {
        row_slice_t row_slice = image_data[row];
        row_slice_t::const_iterator data_it = row_slice.begin();
        for(int col=0; col < cols; ++data_it, col+=1)
        {
            //printf("%.f\n", *data_it);
            image_matrix(row, col) = *data_it;
        } // end "for each column"

    } // end of "for each row"

    return;
}

// Test getLinearAlignEquations
TEST_F(MLPTesting, getLinearAlignEquations) {
  Eigen::MatrixXf A;
  Eigen::MatrixXf b;
  b.setZero();
  c_.set_warp_identity();
  c_.set_c(cv::Point2f(imgSize_/2,imgSize_/2));
  mp_.extractMultilevelPatchFromImage(pyr2_,c_,nLevels_-1,true);
  c_.set_c(cv::Point2f(imgSize_/2+1,imgSize_/2+1));
  // NOTE: only works for patch size = 2, nLevels = 2
  ASSERT_EQ(mpa_.getLinearAlignEquations(pyr2_,mp_,c_,0,nLevels_-1,A,b),true);
  float meanError = (255+255*0.75)/8;
  ASSERT_EQ(b(0),255-meanError);
  ASSERT_EQ(b(1),-meanError);
  ASSERT_EQ(b(2),-meanError);
  ASSERT_EQ(b(3),-meanError);
  ASSERT_EQ(b(4),255*0.75-meanError);
  ASSERT_EQ(b(5),-meanError);
  ASSERT_EQ(b(6),-meanError);
  ASSERT_EQ(b(7),-meanError);
  float mean_diff_dx = 0;
  float mean_diff_dy = 0;
  for(unsigned int l=0;l<nLevels_;l++){
    for(unsigned int i=0;i<patchSize_;i++){
      for(unsigned int j=0;j<patchSize_;j++){
        mean_diff_dx += -pow(0.5,l)*mp_.patches_[l].dx_[i*patchSize_+j];
        mean_diff_dy += -pow(0.5,l)*mp_.patches_[l].dy_[i*patchSize_+j];
      }
    }
  }
  mean_diff_dx /= nLevels_*patchSize_*patchSize_;
  mean_diff_dy /= nLevels_*patchSize_*patchSize_;
  for(unsigned int l=0;l<nLevels_;l++){
    for(unsigned int i=0;i<patchSize_;i++){
      for(unsigned int j=0;j<patchSize_;j++){
        ASSERT_EQ(A(4*l+i*patchSize_+j,0),-pow(0.5,l)*mp_.patches_[l].dx_[i*patchSize_+j]-mean_diff_dx);
        ASSERT_EQ(A(4*l+i*patchSize_+j,1),-pow(0.5,l)*mp_.patches_[l].dy_[i*patchSize_+j]-mean_diff_dy);
      }
    }
  }
}

template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const
{
  matrix.setZero ();

  const unsigned int number_of_points = static_cast <unsigned int> (cloud.points.size ());

  const unsigned int coord[3][2] = {
    {0, 1},
    {0, 2},
    {1, 2}};

  const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
  const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;

  for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
  {
    Eigen::Vector3f point (
      cloud.points[i_point].x,
      cloud.points[i_point].y,
      cloud.points[i_point].z);

    const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
    const float v_length = point (coord[projection][1]) - min[coord[projection][1]];

    const float u_ratio = u_length / u_bin_length;
    unsigned int row = static_cast <unsigned int> (u_ratio);
    if (row == number_of_bins_) row--;

    const float v_ratio = v_length / v_bin_length;
    unsigned int col = static_cast <unsigned int> (v_ratio);
    if (col == number_of_bins_) col--;

    matrix (row, col) += 1.0f;
  }

  matrix /= static_cast <float> (number_of_points);
}

int main(int argc, char **argv) {
	std::vector<color_keyframe::Ptr> frames;

	util U;
	U.load("http://localhost/corridor_map2", frames);
	timestamp_t t0 = get_timestamp();
	std::vector<std::pair<int, int> > overlapping_keyframes;
	int size;
	int workers = argc - 1;
	ros::init(argc, argv, "panorama");

	std::vector<
			actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>*> ac_list;
	for (int i = 0; i < workers; i++) {
		actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>* ac =
				new actionlib::SimpleActionClient<
						rm_multi_mapper::WorkerSlamAction>(
						std::string(argv[i + 1]), true);
		ac_list.push_back(ac);
	}

	sql::ResultSet *res;

	size = frames.size();
	get_pairs(overlapping_keyframes);
	std::vector<rm_multi_mapper::WorkerSlamGoal> goals;
	int keyframes_size = (int) overlapping_keyframes.size();

	for (int k = 0; k < workers; k++) {
		rm_multi_mapper::WorkerSlamGoal goal;

		int last_elem = (keyframes_size / workers) * (k + 1);
		if (k == workers - 1)
			last_elem = keyframes_size;

		for (int i = (keyframes_size / workers) * k; i < last_elem; i++) {
			rm_multi_mapper::KeyframePair keyframe;

			keyframe.first = overlapping_keyframes[i].first;
			keyframe.second = overlapping_keyframes[i].second;
			goal.Overlap.push_back(keyframe);
		}
		goals.push_back(goal);
	}

	ROS_INFO("Waiting for action server to start.");
	for (int i = 0; i < workers; i++) {
		ac_list[i]->waitForServer();
	}

	ROS_INFO("Action server started, sending goal.");

	// send a goal to the action
	for (int i = 0; i < workers; i++) {
		ac_list[i]->sendGoal(goals[i]);
	}

	//wait for the action to return
	std::vector<bool> finished;
	for (int i = 0; i < workers; i++) {
		bool finished_before_timeout = ac_list[i]->waitForResult(
				ros::Duration(30.0));
		finished.push_back(finished_before_timeout);
	}

	bool success = true;
	for (int i = 0; i < workers; i++) {
		success = finished[i] && success;
	}

	Eigen::MatrixXf acc_JtJ;
	acc_JtJ.setZero(size * 6, size * 6);
	Eigen::VectorXf acc_Jte;
	acc_Jte.setZero(size * 6);

	if (success) {

		for (int i = 0; i < workers; i++) {
			Eigen::MatrixXf JtJ;
			JtJ.setZero(size * 6, size * 6);
			Eigen::VectorXf Jte;
			Jte.setZero(size * 6);

			rm_multi_mapper::Vector rosJte = ac_list[i]->getResult()->Jte;
			rm_multi_mapper::Matrix rosJtJ = ac_list[i]->getResult()->JtJ;

			vector2eigen(rosJte, Jte);

			matrix2eigen(rosJtJ, JtJ);

			acc_JtJ += JtJ;
			acc_Jte += Jte;

		}

	} else {
		ROS_INFO("Action did not finish before the time out.");
		std::exit(0);
	}

//.........这里部分代码省略.........

template <typename PointInT, typename GradientT, typename PointOutT> void
pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeRIFT (
      const PointCloudIn &cloud, const PointCloudGradient &gradient, 
      int p_idx, float radius, const std::vector<int> &indices, 
      const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor)
{
  if (indices.empty ())
  {
    PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n");
    return;
  }

  // Determine the number of bins to use based on the size of rift_descriptor
  int nr_distance_bins = static_cast<int> (rift_descriptor.rows ());
  int nr_gradient_bins = static_cast<int> (rift_descriptor.cols ());

  // Get the center point
  pcl::Vector3fMapConst p0 = cloud.points[p_idx].getVector3fMap ();

  // Compute the RIFT descriptor
  rift_descriptor.setZero ();
  for (size_t idx = 0; idx < indices.size (); ++idx)
  {
    // Compute the gradient magnitude and orientation (relative to the center point)
    pcl::Vector3fMapConst point = cloud.points[indices[idx]].getVector3fMap ();
    Eigen::Map<const Eigen::Vector3f> gradient_vector (& (gradient.points[indices[idx]].gradient[0]));

    float gradient_magnitude = gradient_vector.norm ();
    float gradient_angle_from_center = acosf (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude);
    if (!pcl_isfinite (gradient_angle_from_center))
      gradient_angle_from_center = 0.0;

    // Normalize distance and angle values to: 0.0 <= d,g < nr_distances_bins,nr_gradient_bins
    const float eps = std::numeric_limits<float>::epsilon ();
    float d = static_cast<float> (nr_distance_bins) * sqrtf (sqr_distances[idx]) / (radius + eps);
    float g = static_cast<float> (nr_gradient_bins) * gradient_angle_from_center / (static_cast<float> (M_PI) + eps);

    // Compute the bin indices that need to be updated
    int d_idx_min = (std::max)(static_cast<int> (ceil (d - 1)), 0);
    int d_idx_max = (std::min)(static_cast<int> (floor (d + 1)), nr_distance_bins - 1);
    int g_idx_min = static_cast<int> (ceil (g - 1));
    int g_idx_max = static_cast<int> (floor (g + 1));

    // Update the appropriate bins of the histogram 
    for (int g_idx = g_idx_min; g_idx <= g_idx_max; ++g_idx)  
    {
      // Because gradient orientation is cyclical, out-of-bounds values must wrap around
      int g_idx_wrapped = ((g_idx + nr_gradient_bins) % nr_gradient_bins); 

      for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
      {
        // To avoid boundary effects, use linear interpolation when updating each bin 
        float w = (1.0f - fabsf (d - static_cast<float> (d_idx))) * (1.0f - fabsf (g - static_cast<float> (g_idx)));

        rift_descriptor (d_idx, g_idx_wrapped) += w * gradient_magnitude;
      }
    }
  }

  // Normalize the RIFT descriptor to unit magnitude
  rift_descriptor.normalize ();
}