TEST_P(SetTo, Masked)
{
cv::Scalar val = randomScalar(0.0, 255.0);
cv::Mat mat_gold = randomMat(size, type);
cv::Mat mask = randomMat(size, CV_8UC1, 0.0, 2.0);
if (CV_MAT_DEPTH(type) == CV_64F && !supportFeature(devInfo, cv::gpu::NATIVE_DOUBLE))
{
try
{
cv::gpu::GpuMat mat = createMat(size, type, useRoi);
mat.setTo(val, loadMat(mask));
}
catch (const cv::Exception& e)
{
ASSERT_EQ(CV_StsUnsupportedFormat, e.code);
}
}
else
{
cv::gpu::GpuMat mat = loadMat(mat_gold, useRoi);
mat.setTo(val, loadMat(mask, useRoi));
mat_gold.setTo(val, mask);
EXPECT_MAT_NEAR(mat_gold, mat, 0.0);
}
}
TEST_P(SURF, Descriptor)
{
cv::Mat image = readImage("features2d/aloe.png", cv::IMREAD_GRAYSCALE);
ASSERT_FALSE(image.empty());
cv::gpu::SURF_GPU surf;
surf.hessianThreshold = hessianThreshold;
surf.nOctaves = nOctaves;
surf.nOctaveLayers = nOctaveLayers;
surf.extended = extended;
surf.upright = upright;
surf.keypointsRatio = 0.05f;
cv::SURF surf_gold;
surf_gold.hessianThreshold = hessianThreshold;
surf_gold.nOctaves = nOctaves;
surf_gold.nOctaveLayers = nOctaveLayers;
surf_gold.extended = extended;
surf_gold.upright = upright;
if (!supportFeature(devInfo, cv::gpu::GLOBAL_ATOMICS))
{
try
{
std::vector<cv::KeyPoint> keypoints;
cv::gpu::GpuMat descriptors;
surf(loadMat(image), cv::gpu::GpuMat(), keypoints, descriptors);
}
catch (const cv::Exception& e)
{
ASSERT_EQ(CV_StsNotImplemented, e.code);
}
}
else
{
std::vector<cv::KeyPoint> keypoints;
surf_gold(image, cv::noArray(), keypoints);
cv::gpu::GpuMat descriptors;
surf(loadMat(image), cv::gpu::GpuMat(), keypoints, descriptors, true);
cv::Mat descriptors_gold;
surf_gold(image, cv::noArray(), keypoints, descriptors_gold, true);
cv::BFMatcher matcher(cv::NORM_L2);
std::vector<cv::DMatch> matches;
matcher.match(descriptors_gold, cv::Mat(descriptors), matches);
int matchedCount = getMatchedPointsCount(keypoints, keypoints, matches);
double matchedRatio = static_cast<double>(matchedCount) / keypoints.size();
EXPECT_GT(matchedRatio, 0.35);
}
}
TEST_P(SURF, Detector_Masked)
{
cv::Mat image = readImage("features2d/aloe.png", cv::IMREAD_GRAYSCALE);
ASSERT_FALSE(image.empty());
cv::Mat mask(image.size(), CV_8UC1, cv::Scalar::all(1));
mask(cv::Range(0, image.rows / 2), cv::Range(0, image.cols / 2)).setTo(cv::Scalar::all(0));
cv::gpu::SURF_GPU surf;
surf.hessianThreshold = hessianThreshold;
surf.nOctaves = nOctaves;
surf.nOctaveLayers = nOctaveLayers;
surf.extended = extended;
surf.upright = upright;
surf.keypointsRatio = 0.05f;
if (!supportFeature(devInfo, cv::gpu::GLOBAL_ATOMICS))
{
try
{
std::vector<cv::KeyPoint> keypoints;
surf(loadMat(image), loadMat(mask), keypoints);
}
catch (const cv::Exception& e)
{
ASSERT_EQ(CV_StsNotImplemented, e.code);
}
}
else
{
std::vector<cv::KeyPoint> keypoints;
surf(loadMat(image), loadMat(mask), keypoints);
cv::SURF surf_gold;
surf_gold.hessianThreshold = hessianThreshold;
surf_gold.nOctaves = nOctaves;
surf_gold.nOctaveLayers = nOctaveLayers;
surf_gold.extended = extended;
surf_gold.upright = upright;
std::vector<cv::KeyPoint> keypoints_gold;
surf_gold(image, mask, keypoints_gold);
ASSERT_EQ(keypoints_gold.size(), keypoints.size());
int matchedCount = getMatchedPointsCount(keypoints_gold, keypoints);
double matchedRatio = static_cast<double>(matchedCount) / keypoints_gold.size();
EXPECT_GT(matchedRatio, 0.95);
}
}
TEST_P(HistEven, Accuracy)
{
cv::Mat img = readImage("stereobm/aloe-L.png");
ASSERT_FALSE(img.empty());
cv::Mat hsv;
cv::cvtColor(img, hsv, CV_BGR2HSV);
int hbins = 30;
float hranges[] = {0.0f, 180.0f};
std::vector<cv::gpu::GpuMat> srcs;
cv::gpu::split(loadMat(hsv), srcs);
cv::gpu::GpuMat hist;
cv::gpu::histEven(srcs[0], hist, hbins, (int)hranges[0], (int)hranges[1]);
cv::MatND histnd;
int histSize[] = {hbins};
const float* ranges[] = {hranges};
int channels[] = {0};
cv::calcHist(&hsv, 1, channels, cv::Mat(), histnd, 1, histSize, ranges);
cv::Mat hist_gold = histnd;
hist_gold = hist_gold.t();
hist_gold.convertTo(hist_gold, CV_32S);
EXPECT_MAT_NEAR(hist_gold, hist, 0.0);
}
TEST_P(Integral, Accuracy)
{
cv::Mat src = randomMat(size, CV_8UC1);
cv::gpu::GpuMat dst = createMat(cv::Size(src.cols + 1, src.rows + 1), CV_32SC1, useRoi);
cv::gpu::integral(loadMat(src, useRoi), dst);
cv::Mat dst_gold;
cv::integral(src, dst_gold, CV_32S);
EXPECT_MAT_NEAR(dst_gold, dst, 0.0);
}
TEST_P(ReprojectImageTo3D, Accuracy)
{
cv::Mat disp = randomMat(size, depth, 5.0, 30.0);
cv::Mat Q = randomMat(cv::Size(4, 4), CV_32FC1, 0.1, 1.0);
cv::gpu::GpuMat dst;
cv::gpu::reprojectImageTo3D(loadMat(disp, useRoi), dst, Q, 3);
cv::Mat dst_gold;
cv::reprojectImageTo3D(disp, dst_gold, Q, false);
EXPECT_MAT_NEAR(dst_gold, dst, 1e-5);
}
bool ofxRGBDRenderer::setup(string calibrationDirectory){
if(!ofDirectory(calibrationDirectory).exists()){
ofLogError("ofxRGBDRenderer --- Calibration directory doesn't exist: " + calibrationDirectory);
return false;
}
depthCalibration.load(calibrationDirectory+"/depthCalib.yml");
rgbCalibration.load(calibrationDirectory+"/rgbCalib.yml");
loadMat(rotationDepthToRGB, calibrationDirectory+"/rotationDepthToRGB.yml");
loadMat(translationDepthToRGB, calibrationDirectory+"/translationDepthToRGB.yml");
depthToRGBView = ofxCv::makeMatrix(rotationDepthToRGB, translationDepthToRGB);
ofPushView();
rgbCalibration.getDistortedIntrinsics().loadProjectionMatrix();
glGetFloatv(GL_PROJECTION_MATRIX, rgbProjection.getPtr());
ofPopView();
calibrationSetup = true;
return true;
}
void testDetect(const cv::Mat& img)
{
gamma_correction = false;
setSVMDetector(cv::gpu::HOGDescriptor::getDefaultPeopleDetector());
std::vector<cv::Point> locations;
// Test detect
detect(loadMat(img), locations, 0);
#ifdef DUMP
dump(cv::Mat(block_hists), locations);
#else
compare(cv::Mat(block_hists), locations);
#endif
// Test detect on smaller image
cv::Mat img2;
cv::resize(img, img2, cv::Size(img.cols / 2, img.rows / 2));
detect(loadMat(img2), locations, 0);
#ifdef DUMP
dump(cv::Mat(block_hists), locations);
#else
compare(cv::Mat(block_hists), locations);
#endif
// Test detect on greater image
cv::resize(img, img2, cv::Size(img.cols * 2, img.rows * 2));
detect(loadMat(img2), locations, 0);
#ifdef DUMP
dump(cv::Mat(block_hists), locations);
#else
compare(cv::Mat(block_hists), locations);
#endif
}
imageState StereoCapturer::get_frame(Mat & image, double * dt){
cout<<"Running thread "<<runLiveFeed<<endl;
if(live_feed){
cerr<<"Due to processing time limitations, the image cannot be grabbed from a live feed"<<endl;
return CAPTURE_NOT_FOUND;
}
else
if(!ext2.compare("DMX")){
char buffer[255];
if(current > finish){
cout<<"End of frames"<<endl;
return CAPTURE_END_OF_CAPTURE;
}
sprintf(buffer,"%s%i.%s",loadS1.data(),current,ext1.data());
colorFrameStored = cv::imread(buffer,1);
if(colorFrameStored.data == NULL){
cout<< "image "<< buffer<<" not found"<<endl;
return CAPTURE_NOT_FOUND;
}
image = colorFrameStored;
sprintf(buffer,"%s%i.%s",loadS2.data(),current,ext2.data());
if(!loadMat(buffer))
return CAPTURE_NOT_FOUND;
*dt = deltaTimeStored;
current+=increment;
cout<<"Read image "<<loadS1.data()<<current-1<<endl;
return CAPTURE_SUCESSFUL;
}
else{
cerr<<"To Compute the disparity call the StereoCapturer::computeStereo member function "<<endl;
return CAPTURE_NOT_FOUND;
}
}
int main(int argc, char** args){
PetscErrorCode err;
PetscViewer fd = NULL;
Mat invL1 = NULL, invU1 = NULL, invL2 = NULL, invU2 = NULL, H12 = NULL, H21 = NULL;
Vec order = NULL, r = NULL; //, or = NULL; //dimension: n: n1 + n2
Vec seeds = NULL;
// Vec r1 = NULL, q1 = NULL, t1_1 = NULL, t1_2 = NULL, t1_3 = NULL, t1_4 = NULL, t1_5 = NULL; // dimension: n1
// Vec r2 = NULL, q2 = NULL, q_tilda = NULL, t2_1 = NULL, t2_2 = NULL, t2_3 = NULL; // dimension: n2
PetscRandom rand;
PetscLogDouble tic, toc, total_time, time;
PetscInt n, i;
PetscMPIInt rank, size;
PetscInt seed;
PetscScalar c, val;
PetscInt QN = 100;
// Initialize PETSC and MPI
err = PetscInitialize(&argc, &args, (char*) 0, help); CHKERRQ(err);
err = MPI_Comm_size(PETSC_COMM_WORLD, &size); CHKERRQ(err);
err = MPI_Comm_rank(PETSC_COMM_WORLD, &rank); CHKERRQ(err);
err = PetscPrintf(PETSC_COMM_WORLD, "mpi size: %d\n", size); CHKERRQ(err);
// Read matrices and an ordering vector
err = PetscPrintf(PETSC_COMM_WORLD, "Read inputs (invL1, invU1, invL2, invU2, H12, H21, order)\n"); CHKERRQ(err);
err = loadMat("./data/invL1.dat", &invL1, PETSC_COMM_WORLD, MATMPIAIJ, &fd); CHKERRQ(err);
err = checkMat("invL1", invL1); CHKERRQ(err);
err = loadMat("./data/invU1.dat", &invU1, PETSC_COMM_WORLD, MATMPIAIJ, &fd); CHKERRQ(err);
err = checkMat("invU1", invU1); CHKERRQ(err);
err = loadMat("./data/invL2.dat", &invL2, PETSC_COMM_WORLD, MATMPIAIJ, &fd); CHKERRQ(err);
err = checkMat("invL2", invL2); CHKERRQ(err);
err = loadMat("./data/invU2.dat", &invU2, PETSC_COMM_WORLD, MATMPIAIJ, &fd); CHKERRQ(err);
err = checkMat("invU2", invU2); CHKERRQ(err);
err = loadMat("./data/H12.dat", &H12, PETSC_COMM_WORLD, MATMPIAIJ, &fd); CHKERRQ(err);
err = checkMat("H12", H12); CHKERRQ(err);
err = loadMat("./data/H21.dat", &H21, PETSC_COMM_WORLD, MATMPIAIJ, &fd); CHKERRQ(err);
err = checkMat("H21", H21); CHKERRQ(err);
err = loadVec("./data/order.dat", &order, PETSC_COMM_SELF, &fd); CHKERRQ(err); //all processes must have this vector for ordering the result vector.
err = checkVec("order", order); CHKERRQ(err);
// shift -1 for zero-based index
err = VecShift(order, -1); CHKERRQ(err);
err = VecGetSize(order, &n); CHKERRQ(err);
seed = 5;
c = 0.05;
err = PetscTime(&tic); CHKERRQ(err);
//err = BearQueryMat(seed, c, invL1, invU1, invL2, invU2, H12, H21, order); CHKERRQ(err);
//err = PetscTime(&toc); CHKERRQ(err);
//time = toc - tic;
//err = PetscPrintf(PETSC_COMM_WORLD, "running time: %f sec\n", time); CHKERRQ(err);
///* 100 times querying
err = VecCreateSeq(PETSC_COMM_SELF, QN, &seeds); CHKERRQ(err);
err = VecSetFromOptions(seeds); CHKERRQ(err);
err = PetscRandomCreate(PETSC_COMM_WORLD, &rand); CHKERRQ(err);
err = PetscRandomSetSeed(rand, 100); CHKERRQ(err);
err = PetscRandomSetInterval(rand, (PetscScalar) 0, (PetscScalar) n); CHKERRQ(err);
err = PetscRandomSetFromOptions(rand); CHKERRQ(err);
err = VecSetRandom(seeds, rand); CHKERRQ(err);
err = PetscRandomDestroy(&rand); CHKERRQ(err);
seed = 5; //seed is give by user on one-based index
c = 0.05;
i = 0;
err = VecDuplicate(order, &r); CHKERRQ(err);
for(i = 0; i < QN; i++){
err = VecGetValues(seeds, 1, &i, &val);
seed = (PetscInt) val;
//err = PetscPrintf(PETSC_COMM_SELF, "rank: %d, seed: %d\n", rank, seed);
err = PetscTime(&tic); CHKERRQ(err);
err = BearQuery(seed, c, invL1, invU1, invL2, invU2, H12, H21, order, r); CHKERRQ(err);
err = PetscTime(&toc); CHKERRQ(err);
time = toc - tic;
err = PetscPrintf(PETSC_COMM_WORLD, "running time: %f sec\n", time); CHKERRQ(err);
total_time += time;
}
err = PetscPrintf(PETSC_COMM_WORLD, "average running time: %f sec\n", total_time/QN); CHKERRQ(err);
err = VecDestroy(&r);
/* err = MatGetSize(H12, &n1, &n2); CHKERRQ(err);
n = n1 + n2;
err = PetscPrintf(PETSC_COMM_WORLD, "n1: %d, n2: %d\n", n1, n2); CHKERRQ(err);
err = VecCreateMPI(PETSC_COMM_WORLD, PETSC_DECIDE, n, &r); CHKERRQ(err);
err = VecCreateMPI(PETSC_COMM_WORLD, PETSC_DECIDE, n1, &q1); CHKERRQ(err);
err = VecCreateMPI(PETSC_COMM_WORLD, PETSC_DECIDE, n2, &q2); CHKERRQ(err);
err = VecSet(q1, 0); CHKERRQ(err);
err = VecSet(q2, 0); CHKERRQ(err);
seed = seed - 1; // shift -1 for zero-based index
err = VecGetValues(order, 1, &seed, &val); CHKERRQ(err);
oseed = (PetscInt) val;
err = PetscPrintf(PETSC_COMM_WORLD, "Given seed: %d, Reorered seed: %d (0 ~ n-1)\n", seed, oseed); CHKERRQ(err);
if(oseed < n1){
err = VecSetValues(q1, 1, &oseed, &one, INSERT_VALUES); CHKERRQ(err);
}else{
oseed = oseed - n1;
err = VecSetValues(q2, 1, &oseed, &one, INSERT_VALUES); CHKERRQ(err);
//err = printVecSum(q2);
}
err = VecAssemblyBegin(q1); CHKERRQ(err);
err = VecAssemblyBegin(q2); CHKERRQ(err);
err = VecAssemblyEnd(q1); CHKERRQ(err);
err = VecAssemblyEnd(q2); CHKERRQ(err);
err = VecDuplicate(q1, &r1); CHKERRQ(err);
err = VecDuplicate(q1, &t1_1); CHKERRQ(err);
err = VecDuplicate(q1, &t1_2); CHKERRQ(err);
err = VecDuplicate(q1, &t1_3); CHKERRQ(err);
err = VecDuplicate(q1, &t1_4); CHKERRQ(err);
err = VecDuplicate(q1, &t1_5); CHKERRQ(err);
err = VecDuplicate(q2, &r2); CHKERRQ(err);
err = VecDuplicate(q2, &q_tilda); CHKERRQ(err);
err = VecDuplicate(q2, &t2_1); CHKERRQ(err);
err = VecDuplicate(q2, &t2_2); CHKERRQ(err);
err = VecDuplicate(q2, &t2_3); CHKERRQ(err);
// Start matrix-vec multiplications
err = MatMult(invL1, q1, t1_1); CHKERRQ(err);
err = MatMult(invU1, t1_1, t1_2); CHKERRQ(err);
err = MatMult(H21, t1_2, t2_1); CHKERRQ(err);
err = VecAXPBYPCZ(q_tilda, 1.0, -1.0, 0.0, q2, t2_1); CHKERRQ(err);
err = MatMult(invL2, q_tilda, t2_2); CHKERRQ(err);
err = MatMult(invU2, t2_2, r2); CHKERRQ(err);
err = MatMult(H12, r2, t1_3); CHKERRQ(err);
err = VecAXPBYPCZ(t1_4, 1.0, -1.0, 0.0, q1, t1_3); CHKERRQ(err);
err = MatMult(invL1, t1_4, t1_5); CHKERRQ(err);
err = MatMult(invU1, t1_5, r1); CHKERRQ(err);
//err = printVecSum(r1);
//err = VecView(r2, PETSC_VIEWER_STDOUT_WORLD);
// Concatenate r1 and r2
err = VecMerge(r1, r2, r); CHKERRQ(err);
err = VecScale(r, c); CHKERRQ(err);
//err = VecView(r, PETSC_VIEWER_STDOUT_WORLD);
err = VecDuplicate(r, &or); CHKERRQ(err);
err = VecReorder(r, order, or); CHKERRQ(err);
//err = VecView(or, PETSC_VIEWER_STDOUT_WORLD);*/
// Destory matrices and vectors
err = MatDestroy(&invL1); CHKERRQ(err);
err = MatDestroy(&invU1); CHKERRQ(err);
err = MatDestroy(&invL2); CHKERRQ(err);
err = MatDestroy(&invU2); CHKERRQ(err);
err = MatDestroy(&H12); CHKERRQ(err);
err = MatDestroy(&H21); CHKERRQ(err);
err = VecDestroy(&order); CHKERRQ(err);
err = VecDestroy(&r); CHKERRQ(err);
err = VecDestroy(&seeds); CHKERRQ(err);
//err = VecDestroy(&or); CHKERRQ(err);
/* err = VecDestroy(&r1); CHKERRQ(err);
err = VecDestroy(&q1); CHKERRQ(err);
err = VecDestroy(&t1_1); CHKERRQ(err);
err = VecDestroy(&t1_2); CHKERRQ(err);
err = VecDestroy(&t1_3); CHKERRQ(err);
err = VecDestroy(&t1_4); CHKERRQ(err);
err = VecDestroy(&t1_5); CHKERRQ(err);
err = VecDestroy(&r2); CHKERRQ(err);
err = VecDestroy(&q2); CHKERRQ(err);
err = VecDestroy(&q_tilda); CHKERRQ(err);
err = VecDestroy(&t2_1); CHKERRQ(err);
err = VecDestroy(&t2_2); CHKERRQ(err);
err = VecDestroy(&t2_3); CHKERRQ(err);*/
// Finalize
err = PetscFinalize(); CHKERRQ(err);
return 0;
}
TEST_P(BroxOpticalFlow, Regression)
{
cv::Mat frame0 = readImageType("opticalflow/frame0.png", CV_32FC1);
ASSERT_FALSE(frame0.empty());
cv::Mat frame1 = readImageType("opticalflow/frame1.png", CV_32FC1);
ASSERT_FALSE(frame1.empty());
cv::gpu::BroxOpticalFlow brox(0.197f /*alpha*/, 50.0f /*gamma*/, 0.8f /*scale_factor*/,
10 /*inner_iterations*/, 77 /*outer_iterations*/, 10 /*solver_iterations*/);
cv::gpu::GpuMat u;
cv::gpu::GpuMat v;
brox(loadMat(frame0), loadMat(frame1), u, v);
#ifndef DUMP
std::string fname(cvtest::TS::ptr()->get_data_path());
if (devInfo.majorVersion() >= 2)
fname += BROX_OPTICAL_FLOW_DUMP_FILE_CC20;
else
fname += BROX_OPTICAL_FLOW_DUMP_FILE;
std::ifstream f(fname.c_str(), std::ios_base::binary);
int rows, cols;
f.read((char*)&rows, sizeof(rows));
f.read((char*)&cols, sizeof(cols));
cv::Mat u_gold(rows, cols, CV_32FC1);
for (int i = 0; i < u_gold.rows; ++i)
f.read(u_gold.ptr<char>(i), u_gold.cols * sizeof(float));
cv::Mat v_gold(rows, cols, CV_32FC1);
for (int i = 0; i < v_gold.rows; ++i)
f.read(v_gold.ptr<char>(i), v_gold.cols * sizeof(float));
EXPECT_MAT_NEAR(u_gold, u, 0);
EXPECT_MAT_NEAR(v_gold, v, 0);
#else
std::string fname(cvtest::TS::ptr()->get_data_path());
if (devInfo.majorVersion() >= 2)
fname += BROX_OPTICAL_FLOW_DUMP_FILE_CC20;
else
fname += BROX_OPTICAL_FLOW_DUMP_FILE;
std::ofstream f(fname.c_str(), std::ios_base::binary);
f.write((char*)&u.rows, sizeof(u.rows));
f.write((char*)&u.cols, sizeof(u.cols));
cv::Mat h_u(u);
cv::Mat h_v(v);
for (int i = 0; i < u.rows; ++i)
f.write(h_u.ptr<char>(i), u.cols * sizeof(float));
for (int i = 0; i < v.rows; ++i)
f.write(h_v.ptr<char>(i), v.cols * sizeof(float));
#endif
}
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");
}
