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;
}
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;
}
