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