C++ Matrix3f::transposeInPlace方法代码示例

visualization_msgs::Marker SatDetector3DMonitor::getMarker(int marker_id, Eigen::Vector4f centroid, Eigen::Matrix3f covariance_matrix){

  //------- Compute Principal Componets for Marker publishing


  //Get the principal components and the quaternion
  Eigen::Matrix3f evecs;
  Eigen::Vector3f evals;
  //pcl::eigen33 (covariance_matrix, evecs, evals);
  Eigen::EigenSolver<Eigen::Matrix3f> es(covariance_matrix);
	
  evecs = es.eigenvectors().real();
  evals = es.eigenvalues().real();
	    
  Eigen::Matrix3f rotation;
  rotation.row (0) = evecs.col (0);
  rotation.row (1) = evecs.col (1);
  rotation.row (2) = rotation.row (0).cross (rotation.row (1));
	    
  rotation.transposeInPlace ();
  Eigen::Quaternion<float> qt (rotation);
  qt.normalize ();
	    
  //Publish Marker for cluster
  visualization_msgs::Marker marker;	
		
  marker.header.frame_id = base_frame_;
  marker.header.stamp = ros::Time().now();
  marker.ns = "Perception";
  marker.action = visualization_msgs::Marker::ADD;
  marker.id = marker_id;	
  marker.lifetime = ros::Duration(1);	
		
  //centroid position
  marker.type = visualization_msgs::Marker::SPHERE;
  marker.pose.position.x = centroid[0];
  marker.pose.position.y = centroid[1];
  marker.pose.position.z = centroid[2];	
  marker.pose.orientation.x = qt.x();
  marker.pose.orientation.y = qt.y();
  marker.pose.orientation.z = qt.z();
  marker.pose.orientation.w = qt.w();			

  //std::cout << "e1: " << evals(0) << " e2: " << evals(1) << " e3: " << evals(2) << std::endl;

  marker.scale.x = sqrt(evals(0)) * 4;
  marker.scale.y = sqrt(evals(1)) * 4;
  marker.scale.z = sqrt(evals(2)) * 4;
	

  //give it some color!
  marker.color.a = 0.75;
  satToRGB(marker.color.r, marker.color.g, marker.color.b);

  //std::cout << "marker being published" << std::endl;

  return marker;
}

bool Utils::
factorViewMatrix(const Eigen::Projective3f& iMatrix,
                 Eigen::Matrix3f& oCalib, Eigen::Isometry3f& oPose,
                 bool& oIsOrthographic) {
  oIsOrthographic = isOrthographic(iMatrix.matrix());

  // get appropriate rows
  std::vector<int> rows = {0,1,2};
  if (!oIsOrthographic) rows[2] = 3;

  // get A matrix (upper left 3x3) and t vector
  Eigen::Matrix3f A;
  Eigen::Vector3f t;
  for (int i = 0; i < 3; ++i) {
    for (int j = 0; j < 3; ++j) {
      A(i,j) = iMatrix(rows[i],j);
    }
    t[i] = iMatrix(rows[i],3);
  }

  // determine translation vector
  oPose.setIdentity();
  oPose.translation() = -(A.inverse()*t);

  // determine calibration matrix
  Eigen::Matrix3f AAtrans = A*A.transpose();
  AAtrans.col(0).swap(AAtrans.col(2));
  AAtrans.row(0).swap(AAtrans.row(2));
  Eigen::LLT<Eigen::Matrix3f, Eigen::Upper> llt(AAtrans);
  oCalib = llt.matrixU();
  oCalib.col(0).swap(oCalib.col(2));
  oCalib.row(0).swap(oCalib.row(2));
  oCalib.transposeInPlace();

  // compute rotation matrix
  oPose.linear() = (oCalib.inverse()*A).transpose();

  return true;
}

  void get3DMoments(vector<Point> feature_points, int feat_index, int index)
  {
    //    for(int i=0; i<feature_points.size(); i++)
    //   ROS_INFO("%d --> %d,%d",i,feature_points[i].x,feature_points[i].y);
    //ROS_INFO("Getting 3D Moments : %d --> %d,%d", feature_points.size(), width, height);
    
    //Extract the indices for the points in the point cloud data
    pcl::PointIndices point_indices;
     
    for(int i=0; i<feature_points.size(); i++)
      {
	//ROS_INFO("Feature Index : %d, %d",feature_points[i].x, feature_points[i].y);
	point_indices.indices.push_back(feature_points[i].y * width + feature_points[i].x);
      }
    
    //ROS_INFO("Computing 3D Centroid : %d",point_indices.indices.size());
    Eigen::Vector4f centroid;
    Eigen::Matrix3f covariance_matrix;
    
    // Estimate the XYZ centroid
    pcl::compute3DCentroid (pcl_in, point_indices, centroid); 
#ifdef DEBUG
    ROS_INFO("Centroid %d: %f, %f, %f, %f",index,centroid(0),centroid(1),centroid(2),centroid(3));
#endif

    //ROS_INFO("Computing Covariance ");
    //Compute the centroid and the covariance of the points
    computeCovarianceMatrix(pcl_in, point_indices.indices, centroid, covariance_matrix);
    
    //Print the 3D Moments
    //ROS_INFO("Centroid : %f, %f, %f, %f",centroid(0),centroid(1),centroid(2),centroid(3));
#ifdef DEBUG
    std::cout<<"Covariance : "<<std::endl<<covariance_matrix <<std::endl;
#endif

    for(int i=0; i<3; i++)
      {
	feedback_.features[feat_index].moments[index].mean[i] = centroid(i);
	for(int j=0; j<3; j++)
	  {
	    feedback_.features[feat_index].moments[index].covariance[i*3+j] = covariance_matrix(i,j);
	  }
      }

    //Get the principal components and the quaternion
    Eigen::Matrix3f evecs;
    Eigen::Vector3f evals;
    pcl::eigen33 (covariance_matrix, evecs, evals);
    
    Eigen::Matrix3f rotation;
    rotation.row (0) = evecs.col (0);
    rotation.row (1) = evecs.col (1);
    //rotation.row (2) = evecs.col (2);
    rotation.row (2) = rotation.row (0).cross (rotation.row (1));
    //rotation.transposeInPlace ();
#ifdef DEBUG
    std::cerr << "Rotation matrix: " << endl;
    std::cerr << rotation << endl;
    std::cout<<"Eigen vals : "<<evals<<std::endl;
#endif

    rotation.transposeInPlace ();
    Eigen::Quaternion<float> qt (rotation);
    qt.normalize ();

    //Publish Marker
    visualization_msgs::Marker marker;	
    
    marker.header.frame_id = "/openni_rgb_optical_frame";
    marker.header.stamp = ros::Time().now();
    marker.ns = "Triangulation";
    marker.id = index+1;	
    marker.action = visualization_msgs::Marker::ADD;
    marker.lifetime = ros::Duration(5);		
    
    //centroid position
    marker.type = visualization_msgs::Marker::SPHERE;
    
    
    marker.pose.position.x = centroid(0);
    marker.pose.position.y = centroid(1);
    marker.pose.position.z = centroid(2);	
    marker.pose.orientation.x = qt.x();
    marker.pose.orientation.y = qt.y();
    marker.pose.orientation.z = qt.z();
    marker.pose.orientation.w = qt.w();			
    
    marker.scale.x = sqrt(evals(0)) *2;
    marker.scale.y =  sqrt(evals(1)) *2;
    marker.scale.z =  sqrt(evals(2)) *2;
    
    //red
    marker.color.a = 0.5;
    marker.color.r = rand()/((double)RAND_MAX + 1);
    marker.color.g = rand()/((double)RAND_MAX + 1);
    marker.color.b = rand()/((double)RAND_MAX + 1);
    object_pose_marker_pub_.publish(marker);	
    
  }