コード例 #1
0
ファイル: main.cpp プロジェクト: caomw/ParticleFilter-1
int main(int argc, char *argv[]) {
	//Read Map
	Map my_map("/home/icoderaven/CMU/robostats/lab_1/data/map/wean.dat");
	if (!my_map.read_file()) {
		return 0;
	}
	//Display the map
	cv::Mat temp;
	cv::cvtColor(my_map.get_map(), temp, CV_GRAY2BGR);

	cv::imshow("Map", temp);

//	cv::waitKey(-1);
	//Read logs
	LogReader my_logs(
			"/home/icoderaven/CMU/robostats/lab_1/data/log/robotdata2.log");
	if (!my_logs.read_file()) {
		return 0;
	}
	my_logs._lasers[0].sensed_locations();
	Particle::determine_valid_locations(&my_map);
//	for (int i = 0; i < my_logs._lasers.size(); i++) {
//		cv::circle(temp,
//				cv::Point(-my_logs._lasers[i].getX(), -my_logs._lasers[i].getY()),
//				1, CV_RGB(255, 0, 0), 1);
//	}
//	cv::circle(temp, cv::Point(410, 300), 1, CV_RGB(255, 0,0));
	cv::imshow("Tracks", Particle::valid_locations_map);

//Test a particle
//	Particle p(398, 384, M_PI/2.0+0.15, &my_map);
//	p.evaluate_measurement_probability(my_logs._lasers[20], 1);
//	//Test propogation
//	p.markParticle(&temp);
//	for (unsigned int i = 0; i < my_logs._lasers.size() - 1; i++) {
//		cv::cvtColor(my_map.get_map(), temp, CV_GRAY2BGR);
//		p.markParticle(&temp);
////		for (int j = 0; j < 1; j++)
////			p.propogate(my_logs._lasers[i], my_logs._lasers[i+1]).markParticle(
////					&temp);
//		p = p.propogate(my_logs._lasers[i], my_logs._lasers[i+1]);
//		p.markParticle(&temp);
//		cv::imshow("Particle", temp);
//		p.evaluate_measurement_probability(my_logs._lasers[i], 1);
//		cv::waitKey(-1);
//	}
	MCFilter filter(1000, &my_map);
	filter.init();

	for (unsigned int i = 0; i < my_logs._lasers.size()-1; i++) {
		filter.loop(my_logs._lasers[i], my_logs._lasers[i+1]);
		filter.show_particles(&my_map, true, i);
	}
	return 1;
}
コード例 #2
0
ファイル: ep_exscan.cpp プロジェクト: RemiLacroix-IDRIS/XIOS
  int MPI_Exscan(const void *sendbuf, void *recvbuf, int count, MPI_Datatype datatype, MPI_Op op, MPI_Comm comm)
  {
    if(!comm.is_ep)
    {
      return ::MPI_Scan(sendbuf, recvbuf, count, to_mpi_type(datatype), to_mpi_op(op), to_mpi_comm(comm.mpi_comm));
    }
    
    valid_type(datatype);

    int ep_rank = comm.ep_comm_ptr->size_rank_info[0].first;
    int ep_rank_loc = comm.ep_comm_ptr->size_rank_info[1].first;
    int mpi_rank = comm.ep_comm_ptr->size_rank_info[2].first;
    int ep_size = comm.ep_comm_ptr->size_rank_info[0].second;
    int num_ep = comm.ep_comm_ptr->size_rank_info[1].second;
    int mpi_size = comm.ep_comm_ptr->size_rank_info[2].second;

    ::MPI_Aint datasize, lb;
    ::MPI_Type_get_extent(to_mpi_type(datatype), &lb, &datasize);
    
    void* tmp_sendbuf;
    tmp_sendbuf = new void*[datasize * count];

    int my_src = 0;
    int my_dst = ep_rank;

    std::vector<int> my_map(mpi_size, 0);

    for(int i=0; i<comm.rank_map->size(); i++) my_map[comm.rank_map->at(i).second]++;

    for(int i=0; i<mpi_rank; i++) my_src += my_map[i];
    my_src += ep_rank_loc;

     
    for(int i=0; i<mpi_size; i++)
    {
      if(my_dst < my_map[i])
      {
        my_dst = get_ep_rank(comm, my_dst, i); 
        break;
      }
      else
        my_dst -= my_map[i];
    }

    if(ep_rank != my_dst) 
    {
      MPI_Request request[2];
      MPI_Status status[2];

      MPI_Isend(sendbuf,     count, datatype, my_dst, my_dst,  comm, &request[0]);
    
      MPI_Irecv(tmp_sendbuf, count, datatype, my_src, ep_rank, comm, &request[1]);
    
      MPI_Waitall(2, request, status);
    }

    else memcpy(tmp_sendbuf, sendbuf, datasize*count);
    

    void* tmp_recvbuf;
    tmp_recvbuf = new void*[datasize * count];    

    MPI_Reduce_local(tmp_sendbuf, tmp_recvbuf, count, datatype, op, 0, comm);

    if(ep_rank_loc == 0)
      ::MPI_Exscan(MPI_IN_PLACE, tmp_recvbuf, count, to_mpi_type(datatype), to_mpi_op(op), to_mpi_comm(comm.mpi_comm));

    // printf(" ID=%d : %d  %d \n", ep_rank, static_cast<int*>(tmp_recvbuf)[0], static_cast<int*>(tmp_recvbuf)[1]);
    
    MPI_Exscan_local(tmp_sendbuf, tmp_recvbuf, count, datatype, op, comm);

     // printf(" ID=%d : after local tmp_sendbuf = %d %d ; tmp_recvbuf = %d  %d \n", ep_rank, static_cast<int*>(tmp_sendbuf)[0], static_cast<int*>(tmp_sendbuf)[1], static_cast<int*>(tmp_recvbuf)[0], static_cast<int*>(tmp_recvbuf)[1]);



    if(ep_rank != my_src) 
    {
      MPI_Request request[2];
      MPI_Status status[2];

      MPI_Isend(tmp_recvbuf, count, datatype, my_src, my_src,  comm, &request[0]);
    
      MPI_Irecv(recvbuf,     count, datatype, my_dst, ep_rank, comm, &request[1]);
    
      MPI_Waitall(2, request, status);
    }

    else memcpy(recvbuf, tmp_recvbuf, datasize*count);
    



    delete[] tmp_sendbuf;
    delete[] tmp_recvbuf;

  }