void gmsh_render(bool doLabel, int nmpi=0) { std::ofstream pp; pp.open("outgrid.msh"); pp << "$MeshFormat" << std::endl; pp << "2.2 0 8" << std::endl; pp << "$EndMeshFormat" << std::endl; pp << "$Nodes" << std::endl; pp << m_cartx_vert.size() << std::endl; int factor=10000000; for(int i=0; i < m_cartx_vert.size(); ++i){ pp << i+1 << " " << (int)((m_cartx_vert[i])*factor) << " " << (int)((m_carty_vert[i])*factor) << " " << (int)((m_cartz_vert[i])*factor) << std::endl; } pp << "$Elements" << std::endl; int ncell_partition = ncells()/nmpi; if(ncell_partition%4) ncell_partition += (4-ncell_partition%4); pp << ncells() << std::endl; for(int i=0; i < ncells(); ++i) { int partition = i/ncell_partition; pp << i+1 << " " << 2 << " " << 4 << " " << 1 << " " << 1 << " " << 1 << " " << partition << " " << m_vertex_of_cell.at(i+1, 0) << " " << m_vertex_of_cell.at(i+1,1) << " " << m_vertex_of_cell.at(i+1,2) << " " << std::endl; } pp << "$EndElements" << std::endl; }
void TranslationBasedLocalOutlierRejector::process( Size frameSize, InputArray points0, InputArray points1, OutputArray mask) { CV_Assert(points0.type() == points1.type()); CV_Assert(points0.getMat().checkVector(2) == points1.getMat().checkVector(2)); int npoints = points0.getMat().checkVector(2); const Point2f* points0_ = points0.getMat().ptr<Point2f>(); const Point2f* points1_ = points1.getMat().ptr<Point2f>(); mask.create(1, npoints, CV_8U); uchar* mask_ = mask.getMat().ptr<uchar>(); Size ncells((frameSize.width + cellSize_.width - 1) / cellSize_.width, (frameSize.height + cellSize_.height - 1) / cellSize_.height); // fill grid cells grid_.assign(ncells.area(), Cell()); for (int i = 0; i < npoints; ++i) { int cx = std::min(cvRound(points0_[i].x / cellSize_.width), ncells.width - 1); int cy = std::min(cvRound(points0_[i].y / cellSize_.height), ncells.height - 1); grid_[cy * ncells.width + cx].push_back(i); } // process each cell RNG rng(0); int niters = ransacParams_.niters(); std::vector<int> inliers; for (size_t ci = 0; ci < grid_.size(); ++ci) { // estimate translation model at the current cell using RANSAC float x1, y1; const Cell &cell = grid_[ci]; int ninliers, ninliersMax = 0; float dxBest = 0.f, dyBest = 0.f; // find the best hypothesis if (!cell.empty()) { for (int iter = 0; iter < niters; ++iter) { int idx = cell[static_cast<unsigned>(rng) % cell.size()]; float dx = points1_[idx].x - points0_[idx].x; float dy = points1_[idx].y - points0_[idx].y; ninliers = 0; for (size_t i = 0; i < cell.size(); ++i) { x1 = points0_[cell[i]].x + dx; y1 = points0_[cell[i]].y + dy; if (sqr(x1 - points1_[cell[i]].x) + sqr(y1 - points1_[cell[i]].y) < sqr(ransacParams_.thresh)) { ninliers++; } } if (ninliers > ninliersMax) { ninliersMax = ninliers; dxBest = dx; dyBest = dy; } } } // get the best hypothesis inliers ninliers = 0; inliers.resize(ninliersMax); for (size_t i = 0; i < cell.size(); ++i) { x1 = points0_[cell[i]].x + dxBest; y1 = points0_[cell[i]].y + dyBest; if (sqr(x1 - points1_[cell[i]].x) + sqr(y1 - points1_[cell[i]].y) < sqr(ransacParams_.thresh)) { inliers[ninliers++] = cell[i]; } } // refine the best hypothesis dxBest = dyBest = 0.f; for (size_t i = 0; i < inliers.size(); ++i) { dxBest += points1_[inliers[i]].x - points0_[inliers[i]].x; dyBest += points1_[inliers[i]].y - points0_[inliers[i]].y; } if (!inliers.empty()) { dxBest /= inliers.size(); dyBest /= inliers.size(); } // set mask elements for refined model inliers for (size_t i = 0; i < cell.size(); ++i) { x1 = points0_[cell[i]].x + dxBest; y1 = points0_[cell[i]].y + dyBest; if (sqr(x1 - points1_[cell[i]].x) + sqr(y1 - points1_[cell[i]].y) < sqr(ransacParams_.thresh)) { mask_[cell[i]] = 1; } else { mask_[cell[i]] = 0; } } } }
int main(int argc, const char *argv[]) { // parse arguments /////////////////////////////////////////// // Declare the supported options. po::options_description desc("Visualize data"); desc.add_options() ("help", "produce help message") ("width", po::value<double>()->required(), "Width ") ("height", po::value<double>()->required(), "Height ") ("dir", po::value<std::string>()->default_value("."), "Data directory") ; po::positional_options_description pos; pos.add("width", 1); pos.add("height", 1); pos.add("dir", 1); po::variables_map vm; po::store(po::command_line_parser(argc, argv).options(desc).positional(pos).run(), vm); po::notify(vm); double width = vm["width"].as<double>(); double height = vm["height"].as<double>(); std::string datadirectory = vm["dir"].as<std::string>(); // end of parse arguments //////////////////////////////////// cv::Mat laser_pose, laser_ranges, scan_angles; loadMat(laser_pose, datadirectory + "/laser_pose_all.bin"); loadMat(laser_ranges, datadirectory + "/laser_range_all.bin"); loadMat(scan_angles, datadirectory + "/scan_angles_all.bin"); cv::Mat laser_reflectance(laser_ranges.rows, laser_ranges.cols, CV_8U); std::string floorplanfile = datadirectory + "/floorplan.png"; cv::Mat floorplan = cv::imread(floorplanfile, cv::IMREAD_GRAYSCALE); if(! floorplan.data ) // Check for invalid input { std::cout << "Could not open or find the image" << std::endl ; return -1; } cv::transpose(floorplan, floorplan); cv::flip(floorplan, floorplan, 1); cv::Vec2d size_bitmap(width, height); cv::Vec2d margin(1, 1); cv::Vec2d min_pt(- margin(0) - size_bitmap(0)/2, - margin(1) - size_bitmap(1)/2); double max_range = 8; cv::Vec2i gridsize(floorplan.size[0], floorplan.size[1]); cv::Vec2d cellsize; cv::divide(size_bitmap , gridsize, cellsize); //std::cout << cellsize(0) << cellsize(1) << std::endl; cv::Vec2i ncells; cv::divide(min_pt, cellsize, ncells, -2); OccupancyGrid2D<double, int> map( min_pt(0), min_pt(1), cellsize(0), cellsize(1), ncells(0), ncells(1)); // initialize map with occupancy with floorplan for (int r = 0; r < ncells(0); ++r) { for (int c = 0; c < ncells(1); ++c) { int fp_r = r - margin(0) / cellsize(0); int fp_c = c - margin(1) / cellsize(1); if ((0 <= fp_r) && (fp_r < floorplan.rows) && (0 <= fp_c) && (fp_c < floorplan.cols)) { map.og_.at<uint8_t>(r, c) = (floorplan.at<uint8_t>(fp_r, fp_c) > 127) ? map.FREE : map.OCCUPIED; } else { map.og_.at<uint8_t>(r, c) = map.OCCUPIED; } } } boost::mt19937 gen; boost::normal_distribution<> norm_dist(1, NOISE_VARIANCE); boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > norm_rand(gen, norm_dist); for (int r = 0; r < scan_angles.rows; r++) { double* pose = laser_pose.ptr<double>(r); double* angles = scan_angles.ptr<double>(r); double robot_angle = pose[2]; for (int c = 0; c < scan_angles.cols; c++) { double total_angle = robot_angle + angles[c]; cv::Vec2d final_pos; bool refl; double range = map.ray_trace(pose[0], pose[1], total_angle, max_range, final_pos, refl); range *= norm_rand(); laser_ranges.at<double>(r, c) = range; laser_reflectance.at<uint8_t>(r, c) = (uint8_t) refl; } // draw input cv::Mat visin; cv::cvtColor(map.og_, visin, cv::COLOR_GRAY2BGR); cv::Vec2d position(pose[0], pose[1]); map.draw_lasers(visin, position, robot_angle, angles, laser_ranges.ptr<double>(r), laser_reflectance.ptr<uint8_t>(r), scan_angles.cols, CV_RGB(0, 255, 0)); cv::imshow("c", visin); cv::waitKey(33); } saveMat(laser_ranges, "laser_range_all.bin"); saveMat(laser_reflectance, "laser_reflectance_all.bin"); }