C++ MatrixXd::data方法代码示例

 py::EigenDMap<Eigen::Matrix2d> corners() { return py::EigenDMap<Eigen::Matrix2d>(mat.data(),
             py::EigenDStride(mat.outerStride() * (mat.outerSize()-1), mat.innerStride() * (mat.innerSize()-1))); }

// OUT' = bcsr * B' (for matrices)
void A_mul_Bt(Eigen::MatrixXd & out, CSR & csr, Eigen::MatrixXd & B) {
  if (csr.nrow != out.cols()) {throw std::runtime_error("csr.nrow must equal out.cols()");}
  if (csr.ncol != B.cols())   {throw std::runtime_error("csr.ncol must equal b.cols()");}
  if (out.rows() != B.rows()) {throw std::runtime_error("out.rows() must equal B.rows()");}
  csr_A_mul_Bn( out.data(), & csr, B.data(), B.rows() );
}

bool ChompPlanner::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                         const moveit_msgs::MotionPlanRequest& req, const chomp::ChompParameters& params,
                         moveit_msgs::MotionPlanDetailedResponse& res) const
{
  if (!planning_scene)
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No planning scene initialized.");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE;
    return false;
  }

  if (req.start_state.joint_state.position.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state is empty");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  if (not planning_scene->getRobotModel()->satisfiesPositionBounds(req.start_state.joint_state.position.data()))
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Start state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  ros::WallTime start_time = ros::WallTime::now();
  ChompTrajectory trajectory(planning_scene->getRobotModel(), 3.0, .03, req.group_name);

  jointStateToArray(planning_scene->getRobotModel(), req.start_state.joint_state, req.group_name,
                    trajectory.getTrajectoryPoint(0));

  if (req.goal_constraints.empty())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "No goal constraints specified!");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  if (req.goal_constraints[0].joint_constraints.empty())
  {
    ROS_ERROR_STREAM("Only joint-space goals are supported");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GOAL_CONSTRAINTS;
    return false;
  }

  int goal_index = trajectory.getNumPoints() - 1;
  trajectory.getTrajectoryPoint(goal_index) = trajectory.getTrajectoryPoint(0);
  sensor_msgs::JointState js;
  for (const moveit_msgs::JointConstraint& joint_constraint : req.goal_constraints[0].joint_constraints)
  {
    js.name.push_back(joint_constraint.joint_name);
    js.position.push_back(joint_constraint.position);
    ROS_INFO_STREAM_NAMED("chomp_planner", "Setting joint " << joint_constraint.joint_name << " to position "
                                                            << joint_constraint.position);
  }
  jointStateToArray(planning_scene->getRobotModel(), js, req.group_name, trajectory.getTrajectoryPoint(goal_index));

  const moveit::core::JointModelGroup* model_group =
      planning_scene->getRobotModel()->getJointModelGroup(req.group_name);
  // fix the goal to move the shortest angular distance for wrap-around joints:
  for (size_t i = 0; i < model_group->getActiveJointModels().size(); i++)
  {
    const moveit::core::JointModel* model = model_group->getActiveJointModels()[i];
    const moveit::core::RevoluteJointModel* revolute_joint =
        dynamic_cast<const moveit::core::RevoluteJointModel*>(model);

    if (revolute_joint != nullptr)
    {
      if (revolute_joint->isContinuous())
      {
        double start = (trajectory)(0, i);
        double end = (trajectory)(goal_index, i);
        ROS_INFO_STREAM("Start is " << start << " end " << end << " short " << shortestAngularDistance(start, end));
        (trajectory)(goal_index, i) = start + shortestAngularDistance(start, end);
      }
    }
  }

  const std::vector<std::string>& active_joint_names = model_group->getActiveJointModelNames();
  const Eigen::MatrixXd goal_state = trajectory.getTrajectoryPoint(goal_index);
  moveit::core::RobotState goal_robot_state = planning_scene->getCurrentState();
  goal_robot_state.setVariablePositions(
      active_joint_names, std::vector<double>(goal_state.data(), goal_state.data() + active_joint_names.size()));

  if (not goal_robot_state.satisfiesBounds())
  {
    ROS_ERROR_STREAM_NAMED("chomp_planner", "Goal state violates joint limits");
    res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_ROBOT_STATE;
    return false;
  }

  // fill in an initial trajectory based on user choice from the chomp_config.yaml file
  if (params.trajectory_initialization_method_.compare("quintic-spline") == 0)
    trajectory.fillInMinJerk();
  else if (params.trajectory_initialization_method_.compare("linear") == 0)
    trajectory.fillInLinearInterpolation();
  else if (params.trajectory_initialization_method_.compare("cubic") == 0)
    trajectory.fillInCubicInterpolation();
  else if (params.trajectory_initialization_method_.compare("fillTrajectory") == 0)
  {
//.........这里部分代码省略.........

double mp2_gamma_ind_p(
  UnitCell& UCell,
  SuperCell& SCell,
  VMatrixXd& evals,
  std::string mointfile,
  bool read_in_from_file,
  moIntegralFactory& moint_class,
  int argc, char *argv[]
){
  //std::cout << "INITIALIZING MPI...." << std::endl;
  int numtasks, taskid, source;
  //MPI_Init( &argc, &argv );
  MPI_Comm_size( MPI_COMM_WORLD, &numtasks );
  std::cout << "MP2 : RUNNING " << numtasks << " TASKS WITH MPI." << std::endl;
  MPI_Comm_rank( MPI_COMM_WORLD, &taskid );
  std::cout << "MP2 : TASK " << taskid << " STARTED!" << std::endl;
  MPI_Status status;

  int tag1, tag2, tag3, tag4, tag5;
  tag1 = 1;
  tag2 = 2;
  tag3 = 3;
  tag4 = 4;
  tag5 = 5;
  double mp2en = 0.0;
  double total_mp2en = 0.0;
  int nocc, norb;
  std::vector< double > eigenvals;
  Eigen::MatrixXd kernel_matr;
  Eigen::MatrixXd evecsXd;
  std::vector< double > kernel_matr_data;
  std::vector< double > evecsXd_data;
  if( taskid == 0 ){
    nocc = (int)(SCell.nao/2);
    norb = SCell.nao;
    eigenvals.resize( norb );
    for( int i = 0; i < norb; ++i ) eigenvals[ i ] = evals.irrep( 0 )( i, 0 );
    kernel_matr = moint_class.get_kernel_matr();
    evecsXd = moint_class.get_evecsXd();

    /* Send these quantities to all tasks */
    for( int dest = 1; dest < numtasks; ++dest ){
      cout << "SENDING DATA TO TASK " << taskid << endl;
      MPI_Send( &nocc, 1, MPI_INT, dest, tag1, MPI_COMM_WORLD );
      MPI_Send( &norb, 1, MPI_INT, dest, tag2, MPI_COMM_WORLD );
      MPI_Send( &eigenvals[ 0 ], norb, MPI_DOUBLE, dest, tag3, MPI_COMM_WORLD );
      //for( int i = 0; i < norb; ++i ) cout << "TASK " << taskid << " EIGENVALUE " << i << " : " << eigenvals[ i ] << endl;
      MPI_Send( kernel_matr.data(), norb*norb, MPI_DOUBLE, dest, tag4, MPI_COMM_WORLD );
      MPI_Send( evecsXd.data(), norb*norb, MPI_DOUBLE, dest, tag5, MPI_COMM_WORLD );
      cout << "DONE : SENDING DATA TO TASK " << taskid << endl;
    }
  }
  if( taskid > 0 ){
    for( int dest = 1; dest < numtasks; ++dest ){
      if( taskid == dest ){
        cout << "TASK " << dest << " RECIEVING DATA" << endl;
        MPI_Recv( &nocc, 1, MPI_INT, 0, tag1, MPI_COMM_WORLD, &status );
        MPI_Recv( &norb, 1, MPI_INT, 0, tag2, MPI_COMM_WORLD, &status );
        eigenvals.resize( norb );
        kernel_matr_data.resize( norb * norb );
        evecsXd_data.resize( norb * norb );
        MPI_Recv( &eigenvals[ 0 ], norb, MPI_DOUBLE, 0, tag3, MPI_COMM_WORLD, &status );
        //for( int i = 0; i < norb; ++i ) cout << "TASK " << taskid << " EIGENVALUE " << i << " : " << eigenvals[ i ] << endl;
        MPI_Recv( &kernel_matr_data[ 0 ], norb*norb, MPI_DOUBLE, 0, tag4, MPI_COMM_WORLD, &status );
        //for( int i = 0; i < norb*norb; ++i ) cout << "TASK " << taskid << " KERNEL VAL " << i << " : " << kernel_matr_data[ i ] << endl;
        MPI_Recv( &evecsXd_data[ 0 ], norb*norb, MPI_DOUBLE, 0, tag5, MPI_COMM_WORLD, &status );
        //for( int i = 0; i < norb*norb; ++i ) cout << "TASK " << taskid << " EVEVCS_DATA " << i << " : " << evecsXd_data[ i ] << endl;
        kernel_matr.resize( norb, norb );
        evecsXd.resize( norb, norb );
        int counter = 0;
        for( int i = 0; i < norb; ++i )
        for( int j = 0; j < norb; ++j, counter++ ){
          kernel_matr( j, i ) = kernel_matr_data[ counter ];
          evecsXd( j, i ) = evecsXd_data[ counter ];
        }
        //cout << taskid << " KERNEL MATRIX : " << endl;
        //cout << kernel_matr.block( 0, 0, 3, 3 ) << endl;
        //cout << taskid << " EVECS MATRIX  : " << endl;
        //cout << evecsXd.block( 0, 0, 3, 3 ) << endl;
        cout << "DONE : TASK " << dest << " RECIEVING DATA" << endl;
      }
    }
  }

  MPI_Barrier( MPI_COMM_WORLD );

  int mo_index_in_arr;
  double denom, moint1, moint2;
  size_t moindex1, moindex2;
  std::vector< size_t >::iterator it;
  bool sort_mo = true;

  clock_t t, timer;
  size_t opt_size_arr, size_arr;
  std::vector< size_t > indx_arr;
  std::vector< double > dble_arr;
  bool optimize_size_arr;


  if( read_in_from_file ){
//.........这里部分代码省略.........

void CloudAnalyzer2D::examineFreeSpaceEvidence() {
  freeSpaceEvidence.clear();
  Eigen::Vector3f cameraCenter = -1.0 * pointMin;

  voxel::DirectVoxel<char> freeSpace(numX, numY, numZ);

  for (int k = 0; k < numZ; ++k) {
    for (int j = 0; j < numY; ++j) {
      for (int i = 0; i < numX; ++i) {

        if (!pointInVoxel->at(i, j, k))
          continue;

        Eigen::Vector3d ray(i, j, k);
        ray[0] -= cameraCenter[0] * FLAGS_scale;
        ray[1] -= cameraCenter[1] * FLAGS_scale;
        ray[2] -= cameraCenter[2] * zScale;
        double length = ray.norm();
        Eigen::Vector3d unitRay = ray / length;

        Eigen::Vector3i voxelHit;
        for (int a = 0; a <= ceil(length); ++a) {
          voxelHit[0] = floor(cameraCenter[0] * FLAGS_scale + a * unitRay[0]);
          voxelHit[1] = floor(cameraCenter[1] * FLAGS_scale + a * unitRay[1]);
          voxelHit[2] = floor(cameraCenter[2] * zScale + a * unitRay[2]);

          if (voxelHit[0] < 0 || voxelHit[0] >= numX)
            continue;
          if (voxelHit[1] < 0 || voxelHit[1] >= numY)
            continue;
          if (voxelHit[2] < 0 || voxelHit[2] >= numZ)
            continue;

          freeSpace(voxelHit) = 1;
        }
      }
    }
  }

  for (int r = 0; r < R->size(); ++r) {
    Eigen::MatrixXd collapsedCount = Eigen::MatrixXd::Zero(newRows, newCols);

#pragma omp parallel for schedule(static)
    for (int i = 0; i < collapsedCount.cols(); ++i) {
      for (int j = 0; j < collapsedCount.rows(); ++j) {
        for (int k = 0; k < numZ; ++k) {
          const Eigen::Vector3d coord(i, j, k);
          const Eigen::Vector3i src =
              (R->at(r) * (coord - newZZ) + zeroZero)
                  .unaryExpr([](auto v) { return std::round(v); })
                  .cast<int>();

          if (src[0] < 0 || src[0] >= numX || src[1] < 0 || src[1] >= numY ||
              src[2] < 0 || src[2] >= numZ)
            continue;

          if (freeSpace(src))
            ++collapsedCount(j, i);
        }
      }
    }

    double average, sigma;
    const double *vPtr = collapsedCount.data();
    std::tie(average, sigma) = place::aveAndStdev(
        vPtr, vPtr + collapsedCount.size(), [](double v) { return v; },
        [](double v) -> bool { return v; });

    cv::Mat heatMap(newRows, newCols, CV_8UC1, cv::Scalar::all(255));
    for (int j = 0; j < heatMap.rows; ++j) {
      uchar *dst = heatMap.ptr<uchar>(j);
      for (int i = 0; i < heatMap.cols; ++i) {
        const double count = collapsedCount(j, i);
        if (count > 0) {
          const int gray = cv::saturate_cast<uchar>(
              255.0 * ((count - average) / sigma + 1.0));
          dst[i] = 255 - gray;
        }
      }
    }
    const double radius = 0.3;
    for (int j = -sqrt(radius) * FLAGS_scale; j < sqrt(radius) * FLAGS_scale;
         ++j) {
      uchar *dst = heatMap.ptr<uchar>(j + imageZeroZero[1]);
      for (int i = -sqrt(radius * FLAGS_scale * FLAGS_scale - j * j);
           i < sqrt(radius * FLAGS_scale * FLAGS_scale - j * j); ++i) {
        dst[i + imageZeroZero[0]] = 0;
      }
    }

    if (FLAGS_preview) {
      cvNamedWindow("Preview", CV_WINDOW_NORMAL);
      cv::imshow("Preview", heatMap);
      cv::waitKey(0);
    }

    freeSpaceEvidence.push_back(heatMap);
  }
}