void Pointcloud::crop(point3d lowerBound, point3d upperBound) {
Pointcloud result;
float min_x, min_y, min_z;
float max_x, max_y, max_z;
float x,y,z;
min_x = lowerBound(0); min_y = lowerBound(1); min_z = lowerBound(2);
max_x = upperBound(0); max_y = upperBound(1); max_z = upperBound(2);
for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
x = (*it)(0);
y = (*it)(1);
z = (*it)(2);
if ( (x >= min_x) &&
(y >= min_y) &&
(z >= min_z) &&
(x <= max_x) &&
(y <= max_y) &&
(z <= max_z) ) {
result.push_back (x,y,z);
}
} // end for points
this->clear();
this->push_back(result);
}
int main(int argc, char** argv) {
if (argc != 3)
printUsage(argv[0]);
string inputFilename = argv[1];
string outputFilename = argv[2];
// pcl point cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr pclcloud( new pcl::PointCloud<pcl::PointXYZ>() );
pcl::io::loadPCDFile( inputFilename, *pclcloud );
// data conversion
Pointcloud * cloud = new Pointcloud;
for ( size_t i = 0; i < pclcloud->size(); ++ i ) {
point3d pt(pclcloud->points[i].x, pclcloud->points[i].y, pclcloud->points[i].z);
cloud->push_back( pt );
}
point3d sensor_origin(0,0,0);
OcTree* tree = new OcTree(0.1);
tree->insertPointCloud( cloud, sensor_origin );
tree->writeBinary(outputFilename);
}
int Test::run(){
int error = 0;
unsigned int size = 20;
Map testMap("testMap.map", size,size);
if(testMap.getMapContent().size() != size) error++;
for(int y = 0; y < testMap.height; y++){
for(int x = 0; x < testMap.width; x++){
if(testMap.getMapContent()[y].size() != size) error++;
if(testMap.getMapObject(y,x) != 0) error++;
testMap.setMapObject(1,y,x);
}
}
for(int y = 0; y < testMap.height; y++){
for(int x = 0; x < testMap.width; x++){
if(testMap.getMapObject(y,x) != 1) error++;
}
}
testMap.setMapObject(0,size - 5, size - 2);
if(testMap.getMapObject(size - 5,size - 2) != 0) error++;
testMap.setMapObject(1,size - size,size - size);
if(testMap.getMapObject(size - size,size -size) != 1) error++;
testMap.setMapObject(1,size - 1,size - (size - 1));
if(testMap.getMapObject(size - 1,size - (size - 1)) != 1) error++;
testMap.setMapObject(0,size - 1,size - 1);
if(testMap.getMapObject(size - 1,size - 1) != 0) error++;
for(int y = 0; y < testMap.height; y++){
for(int x = 0; x < testMap.width; x++){
testMap.setMapObject(0,y,x);
}
}
testMap.saveMap();
testMap.setMapObject(3,6,2);
testMap.setMapObject(1,9,1);
SimulateMap testSim(&testMap);
testSim.setScanPoint(6,2);
testSim.simulate();
Pointcloud pC = testSim.getPointCloud();
for(Pointcloud::Point p : pC.getPoints()){
if(p.X != -1) error++;
if(p.Y != -3) error++;
}
return error;
}
void Pointcloud::minDist(double thres) {
Pointcloud result;
float x,y,z;
for (Pointcloud::const_iterator it=begin(); it!=end(); it++) {
x = (*it)(0);
y = (*it)(1);
z = (*it)(2);
double dist = sqrt(x*x+y*y+z*z);
if ( dist > thres ) result.push_back (x,y,z);
} // end for points
this->clear();
this->push_back(result);
}
Pointcloud SimulateMap::addNoise(Pointcloud pc){
Pointcloud pNC;
for(Pointcloud::Point p : *pc.getPoints()){
pNC.setPoint(p);
}
pNC.offset = pc.offset;
pNC.orientation = pc.orientation;
int cPx = checkpoints[0].getX();
int cPy = checkpoints[0].getY();
for(int i = 0; i < pNC.getPoints()->size(); ++i){
int pCx = pNC.getPoints()->at(i).X;
int pCy = pNC.getPoints()->at(i).Y;
int distanceX = (pCx - cPx)^2;
int distanceY = (pCy - cPy)^2;
double distance = sqrt(distanceX - distanceY);
if(distance > (Values::NOISETHRESHOLD/10)){
//std::cout << "Point distance: " << distance << "\n";
int intDistance = static_cast<int>(distance * 10);
int randomXValue = (rand()% (intDistance * 2)) - intDistance;
int randomYValue = (rand()% (intDistance * 2)) - intDistance;
randomXValue = randomXValue / 10;
randomYValue = randomYValue / 10;
//std::cout << "Random distance: " << randomXValue <<" , " << randomYValue <<"\n";
Pointcloud::Point newPosition = pNC.getPoints()->at(i);
newPosition.X += randomXValue;
newPosition.Y += randomYValue;
pNC.getPoints()->at(i) = newPosition;
}
}
return pNC;
}
int main(int argc, char** argv) {
//##############################################################
OcTree tree (0.05);
tree.enableChangeDetection(true);
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (4.01f,0.01f,0.01f);
tree.insertRay(origin, point_on_surface);
printChanges(tree);
tree.updateNode(point3d(2.01f, 0.01f, 0.01f), 2.0f);
printChanges(tree);
tree.updateNode(point3d(2.01f, 0.01f, 0.01f), -2.0f);
printChanges(tree);
cout << "generating spherical scan at " << origin << " ..." << endl;
for (int i=-100; i<101; i++) {
Pointcloud cloud;
for (int j=-100; j<101; j++) {
point3d rotated = point_on_surface;
rotated.rotate_IP(0, DEG2RAD(i*0.5), DEG2RAD(j*0.5));
cloud.push_back(rotated);
}
// insert in global coordinates:
tree.insertPointCloud(cloud, origin, -1);
}
printChanges(tree);
cout << "done." << endl;
return 0;
}
void Pointcloud::subSampleRandom(unsigned int num_samples, Pointcloud& sample_cloud) {
point3d_collection samples;
// visual studio does not support random_sample_n
#ifdef _MSC_VER
samples.reserve(this->size());
samples.insert(samples.end(), this->begin(), this->end());
std::random_shuffle(samples.begin(), samples.end());
samples.resize(num_samples);
#else
random_sample_n(begin(), end(), std::back_insert_iterator<point3d_collection>(samples), num_samples);
for (unsigned int i=0; i<samples.size(); i++) {
sample_cloud.push_back(samples[i]);
}
#endif
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
// Usage:
// Constructors/Destructor:
// octree = octomapWrapper(resolution); // constructor: new tree with
// specified resolution
// octree = octomapWrapper(filename); // constructor: load from file
// octomapWrapper(octree); // destructor
//
// Queries:
// results = octomapWrapper(octree, 1, pts) // search
// leaf_nodes = octomapWrapper(octree, 2) // getLeafNodes
//
// Update tree:
// octomapWrapper(octree, 11, pts, occupied) // updateNote(pts, occupied).
// pts is 3-by-n, occupied is 1-by-n logical
//
// General operations:
// octomapWrapper(octree, 21, filename) // save to file
OcTree* tree = NULL;
if (nrhs == 1) {
if (mxIsNumeric(prhs[0])) { // constructor w/ resolution
if (nlhs > 0) {
double resolution = mxGetScalar(prhs[0]);
// mexPrintf("Creating octree w/ resolution %f\n", resolution);
tree = new OcTree(resolution);
plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
}
} else if (mxIsChar(prhs[0])) {
if (nlhs > 0) {
char* filename = mxArrayToString(prhs[0]);
// mexPrintf("Loading octree from %s\n", filename);
tree = new OcTree(filename);
plhs[0] = createDrakeMexPointer((void*)tree, "OcTree");
mxFree(filename);
}
} else { // destructor. note: assumes prhs[0] is a DrakeMexPointer (todo:
// could check)
// mexPrintf("Deleting octree\n");
destroyDrakeMexPointer<OcTree*>(prhs[0]);
}
return;
}
tree = (OcTree*)getDrakeMexPointer(prhs[0]);
int COMMAND = (int)mxGetScalar(prhs[1]);
switch (COMMAND) {
case 1: // search
{
mexPrintf("octree search\n");
if (mxGetM(prhs[2]) != 3)
mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
int n = mxGetN(prhs[2]);
double* pts = mxGetPrSafe(prhs[2]);
if (nlhs > 0) {
plhs[0] = mxCreateDoubleMatrix(1, n, mxREAL);
double* presults = mxGetPrSafe(plhs[0]);
for (int i = 0; i < n; i++) {
OcTreeNode* result =
tree->search(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2]);
if (result == NULL)
presults[i] = -1.0;
else
presults[i] = result->getOccupancy();
}
}
} break;
case 2: // get leaf nodes
{
// mexPrintf("octree get leaf nodes\n");
int N = tree->getNumLeafNodes();
plhs[0] = mxCreateDoubleMatrix(3, N, mxREAL);
double* leaf_xyz = mxGetPrSafe(plhs[0]);
double* leaf_value = NULL, * leaf_size = NULL;
if (nlhs > 1) { // return value
plhs[1] = mxCreateDoubleMatrix(1, N, mxREAL);
leaf_value = mxGetPrSafe(plhs[1]);
}
if (nlhs > 2) { // return size
plhs[2] = mxCreateDoubleMatrix(1, N, mxREAL);
leaf_size = mxGetPrSafe(plhs[2]);
}
for (OcTree::leaf_iterator leaf = tree->begin_leafs(),
end = tree->end_leafs();
leaf != end; ++leaf) {
leaf_xyz[0] = leaf.getX();
leaf_xyz[1] = leaf.getY();
leaf_xyz[2] = leaf.getZ();
leaf_xyz += 3;
if (leaf_value) *leaf_value++ = leaf->getValue();
if (leaf_size) *leaf_size++ = leaf.getSize();
}
} break;
case 11: // add occupied pts
{
// mexPrintf("octree updateNode\n");
if (mxGetM(prhs[2]) != 3)
mexErrMsgTxt("octomapWrapper: pts must be 3-by-n");
int n = mxGetN(prhs[2]);
double* pts = mxGetPrSafe(prhs[2]);
mxLogical* occupied = mxGetLogicals(prhs[3]);
for (int i = 0; i < n; i++) {
tree->updateNode(pts[3 * i], pts[3 * i + 1], pts[3 * i + 2],
occupied[i]);
}
} break;
case 12: // insert a scan of endpoints and sensor origin
{
// pointsA should be 3xN, originA is 3x1
double* points = mxGetPrSafe(prhs[2]);
double* originA = mxGetPrSafe(prhs[3]);
int n = mxGetN(prhs[2]);
point3d origin((float)originA[0], (float)originA[1], (float)originA[2]);
Pointcloud pointCloud;
for (int i = 0; i < n; i++) {
point3d point((float)points[3 * i], (float)points[3 * i + 1],
(float)points[3 * i + 2]);
pointCloud.push_back(point);
}
tree->insertPointCloud(pointCloud, origin);
} break;
case 21: // save to file
{
char* filename = mxArrayToString(prhs[2]);
// mexPrintf("writing octree to %s\n", filename);
tree->writeBinary(filename);
mxFree(filename);
} break;
default:
mexErrMsgTxt("octomapWrapper: Unknown command");
}
}
int main(int argc, char** argv) {
// default values:
double res = 0.1;
if (argc < 2)
printUsage(argv[0]);
string graphFilename = std::string(argv[1]);
double maxrange = -1;
int max_scan_no = -1;
int skip_scan_eval = 5;
int arg = 1;
while (++arg < argc) {
if (! strcmp(argv[arg], "-i"))
graphFilename = std::string(argv[++arg]);
else if (! strcmp(argv[arg], "-res"))
res = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-m"))
maxrange = atof(argv[++arg]);
else if (! strcmp(argv[arg], "-n"))
max_scan_no = atoi(argv[++arg]);
else {
printUsage(argv[0]);
}
}
cout << "\nReading Graph file\n===========================\n";
ScanGraph* graph = new ScanGraph();
if (!graph->readBinary(graphFilename))
exit(2);
size_t num_points_in_graph = 0;
if (max_scan_no > 0) {
num_points_in_graph = graph->getNumPoints(max_scan_no-1);
cout << "\n Data points in graph up to scan " << max_scan_no << ": " << num_points_in_graph << endl;
}
else {
num_points_in_graph = graph->getNumPoints();
cout << "\n Data points in graph: " << num_points_in_graph << endl;
}
cout << "\nCreating tree\n===========================\n";
OcTree* tree = new OcTree(res);
size_t numScans = graph->size();
unsigned int currentScan = 1;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (currentScan % skip_scan_eval != 0){
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
tree->insertPointCloud(**scan_it, maxrange);
} else
cout << "(SKIP) " << flush;
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
tree->expand();
cout << "\nEvaluating scans\n===========================\n";
currentScan = 1;
size_t num_points = 0;
size_t num_voxels_correct = 0;
size_t num_voxels_wrong = 0;
size_t num_voxels_unknown = 0;
for (ScanGraph::iterator scan_it = graph->begin(); scan_it != graph->end(); scan_it++) {
if (currentScan % skip_scan_eval == 0){
if (max_scan_no > 0) cout << "("<<currentScan << "/" << max_scan_no << ") " << flush;
else cout << "("<<currentScan << "/" << numScans << ") " << flush;
pose6d frame_origin = (*scan_it)->pose;
point3d sensor_origin = frame_origin.inv().transform((*scan_it)->pose.trans());
// transform pointcloud:
Pointcloud scan (*(*scan_it)->scan);
scan.transform(frame_origin);
point3d origin = frame_origin.transform(sensor_origin);
KeySet free_cells, occupied_cells;
tree->computeUpdate(scan, origin, free_cells, occupied_cells, maxrange);
num_points += scan.size();
// count free cells
for (KeySet::iterator it = free_cells.begin(); it != free_cells.end(); ++it) {
OcTreeNode* n = tree->search(*it);
if (n){
if (tree->isNodeOccupied(n))
num_voxels_wrong++;
else
num_voxels_correct++;
} else
num_voxels_unknown++;
} // count occupied cells
for (KeySet::iterator it = occupied_cells.begin(); it != occupied_cells.end(); ++it) {
OcTreeNode* n = tree->search(*it);
if (n){
if (tree->isNodeOccupied(n))
num_voxels_correct++;
else
num_voxels_wrong++;
} else
num_voxels_unknown++;
}
}
if ((max_scan_no > 0) && (currentScan == (unsigned int) max_scan_no))
break;
currentScan++;
}
cout << "\nFinished evaluating " << num_points <<"/"<< num_points_in_graph << " points.\n"
<<"Voxels correct: "<<num_voxels_correct<<" #wrong: " <<num_voxels_wrong << " #unknown: " <<num_voxels_unknown
<<". % correct: "<< num_voxels_correct/double(num_voxels_correct+num_voxels_wrong)<<"\n\n";
delete graph;
delete tree;
return 0;
}
int main(int argc, char** argv) {
if (argc != 2){
std::cerr << "Error: you need to specify a test as argument" << std::endl;
return 1; // exit 1 means failure
}
std::string test_name (argv[1]);
// ------------------------------------------------------------
if (test_name == "MathVector") {
// test constructors
Vector3* twos = new Vector3();
Vector3* ones = new Vector3(1,1,1);
for (int i=0;i<3;i++) {
(*twos)(i) = 2;
}
// test basic operations
Vector3 subtraction = *twos - *ones;
Vector3 addition = *twos + *ones;
Vector3 multiplication = *twos * 2.;
for (int i=0;i<3;i++) {
EXPECT_FLOAT_EQ (subtraction(i), 1.);
EXPECT_FLOAT_EQ (addition(i), 3.);
EXPECT_FLOAT_EQ (multiplication(i), 4.);
}
// copy constructor
Vector3 rotation = *ones;
// rotation
rotation.rotate_IP (M_PI, 1., 0.1);
EXPECT_FLOAT_EQ (rotation.x(), 1.2750367);
EXPECT_FLOAT_EQ (rotation.y(), (-1.1329513));
EXPECT_FLOAT_EQ (rotation.z(), 0.30116868);
// ------------------------------------------------------------
} else if (test_name == "MathPose") {
// constructors
Pose6D a (1.0f, 0.1f, 0.1f, 0.0f, 0.1f, (float) M_PI/4. );
Pose6D b;
Vector3 trans(1.0f, 0.1f, 0.1f);
Quaternion rot(0.0f, 0.1f, (float) M_PI/4.);
Pose6D c(trans, rot);
// comparator
EXPECT_TRUE ( a == c);
// toEuler
EXPECT_FLOAT_EQ (c.yaw() , M_PI/4.);
// transform
Vector3 t = c.transform (trans);
EXPECT_FLOAT_EQ (t.x() , 1.6399229);
EXPECT_FLOAT_EQ (t.y() , 0.8813442);
EXPECT_FLOAT_EQ (t.z() , 0.099667005);
// inverse transform
Pose6D c_inv = c.inv();
Vector3 t2 = c_inv.transform (t);
EXPECT_FLOAT_EQ (t2.x() , trans.x());
EXPECT_FLOAT_EQ (t2.y() , trans.y());
EXPECT_FLOAT_EQ (t2.z() , trans.z());
// ------------------------------------------------------------
} else if (test_name == "InsertRay") {
double p = 0.5;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
p = 0.1;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
p = 0.99;
EXPECT_FLOAT_EQ(p, probability(logodds(p)));
float l = 0;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
l = -4;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
l = 2;
EXPECT_FLOAT_EQ(l, logodds(probability(l)));
OcTree tree (0.05);
tree.setProbHit(0.7);
tree.setProbMiss(0.4);
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (2.01f,0.01f,0.01f);
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
EXPECT_TRUE (tree.insertRay(origin, origin+point_on_surface));
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
EXPECT_TRUE (tree.writeBinary("sphere_rays.bt"));
EXPECT_EQ ((int) tree.size(), 50615);
// ------------------------------------------------------------
// ray casting is now in "test_raycasting.cpp"
// ------------------------------------------------------------
// insert scan test
// insert graph node test
// write graph test
} else if (test_name == "InsertScan") {
Pointcloud* measurement = new Pointcloud();
point3d origin (0.01f, 0.01f, 0.02f);
point3d point_on_surface (2.01f, 0.01f, 0.01f);
for (int i=0; i<360; i++) {
for (int j=0; j<360; j++) {
point3d p = origin+point_on_surface;
measurement->push_back(p);
point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
}
point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
}
OcTree tree (0.05);
tree.insertPointCloud(*measurement, origin);
EXPECT_EQ (tree.size(), 53959);
ScanGraph* graph = new ScanGraph();
Pose6D node_pose (origin.x(), origin.y(), origin.z(),0.0f,0.0f,0.0f);
graph->addNode(measurement, node_pose);
EXPECT_TRUE (graph->writeBinary("test.graph"));
delete graph;
// ------------------------------------------------------------
// graph read file test
} else if (test_name == "ReadGraph") {
// not really meaningful, see better test in "test_scans.cpp"
ScanGraph graph;
EXPECT_TRUE (graph.readBinary("test.graph"));
// ------------------------------------------------------------
} else if (test_name == "StampedTree") {
OcTreeStamped stamped_tree (0.05);
// fill tree
for (int x=-20; x<20; x++)
for (int y=-20; y<20; y++)
for (int z=-20; z<20; z++) {
point3d p ((float) x*0.05f+0.01f, (float) y*0.05f+0.01f, (float) z*0.05f+0.01f);
stamped_tree.updateNode(p, true); // integrate 'occupied' measurement
}
// test if update times set
point3d query (0.1f, 0.1f, 0.1f);
OcTreeNodeStamped* result = stamped_tree.search (query);
EXPECT_TRUE (result);
unsigned int tree_time = stamped_tree.getLastUpdateTime();
unsigned int node_time = result->getTimestamp();
std::cout << "After 1st update (cube): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp() << std::endl;
EXPECT_TRUE (tree_time > 0);
#ifdef _WIN32
Sleep(1000);
#else
sleep(1);
#endif
stamped_tree.integrateMissNoTime(result); // reduce occupancy, no time update
std::cout << "After 2nd update (single miss): Tree time " <<tree_time << "; node(0.1, 0.1, 0.1) time " << node_time << std::endl;
EXPECT_EQ (node_time, result->getTimestamp()); // node time updated?
point3d query2 = point3d (0.1f, 0.1f, 0.3f);
stamped_tree.updateNode(query2, true); // integrate 'occupied' measurement
OcTreeNodeStamped* result2 = stamped_tree.search (query2);
EXPECT_TRUE (result2);
result = stamped_tree.search (query);
EXPECT_TRUE (result);
std::cout << "After 3rd update (single hit at (0.1, 0.1, 0.3): Tree time " << stamped_tree.getLastUpdateTime() << "; node(0.1, 0.1, 0.1) time " << result->getTimestamp()
<< "; node(0.1, 0.1, 0.3) time " << result2->getTimestamp() << std::endl;
EXPECT_TRUE (result->getTimestamp() < result2->getTimestamp()); // result2 has been updated
EXPECT_EQ(result2->getTimestamp(), stamped_tree.getLastUpdateTime());
// ------------------------------------------------------------
} else if (test_name == "OcTreeKey") {
OcTree tree (0.05);
point3d p(0.0,0.0,0.0);
OcTreeKey key;
tree.coordToKeyChecked(p, key);
point3d p_inv = tree.keyToCoord(key);
EXPECT_FLOAT_EQ (0.025, p_inv.x());
EXPECT_FLOAT_EQ (0.025, p_inv.y());
EXPECT_FLOAT_EQ (0.025, p_inv.z());
// ------------------------------------------------------------
} else {
std::cerr << "Invalid test name specified: " << test_name << std::endl;
return 1;
}
std::cerr << "Test successful.\n";
return 0;
}
void Pointcloud::push_back(const Pointcloud& other) {
for (Pointcloud::const_iterator it = other.begin(); it != other.end(); it++) {
points.push_back(point3d(*it));
}
}
void scan3d::reconstruct_model_simple(Pointcloud & pointcloud, CalibrationData const& calib,
cv::Mat const& pattern_image, cv::Mat const& min_max_image, cv::Mat const& color_image,
cv::Size const& projector_size, int threshold, double max_dist, QWidget * parent_widget)
{
if (!pattern_image.data || pattern_image.type()!=CV_32FC2)
{ //pattern not correctly decoded
std::cerr << "[reconstruct_model] ERROR invalid pattern_image\n";
return;
}
if (!min_max_image.data || min_max_image.type()!=CV_8UC2)
{ //pattern not correctly decoded
std::cerr << "[reconstruct_model] ERROR invalid min_max_image\n";
return;
}
if (color_image.data && color_image.type()!=CV_8UC3)
{ //not standard RGB image
std::cerr << "[reconstruct_model] ERROR invalid color_image\n";
return;
}
if (!calib.is_valid())
{ //invalid calibration
return;
}
//parameters
//const unsigned threshold = config.value("main/shadow_threshold", 70).toUInt();
//const double max_dist = config.value("main/max_dist_threshold", 40).toDouble();
//const bool remove_background = config.value("main/remove_background", true).toBool();
//const double plane_dist = config.value("main/plane_dist", 100.0).toDouble();
double plane_dist = 100.0;
/* background removal
cv::Point2i plane_coord[3];
plane_coord[0] = cv::Point2i(config.value("background_plane/x1").toUInt(), config.value("background_plane/y1").toUInt());
plane_coord[1] = cv::Point2i(config.value("background_plane/x2").toUInt(), config.value("background_plane/y2").toUInt());
plane_coord[2] = cv::Point2i(config.value("background_plane/x3").toUInt(), config.value("background_plane/y3").toUInt());
if (plane_coord[0].x<=0 || plane_coord[0].x>=pattern_local.cols
|| plane_coord[0].y<=0 || plane_coord[0].y>=pattern_local.rows)
{
plane_coord[0] = cv::Point2i(50, 50);
config.setValue("background_plane/x1", plane_coord[0].x);
config.setValue("background_plane/y1", plane_coord[0].y);
}
if (plane_coord[1].x<=0 || plane_coord[1].x>=pattern_local.cols
|| plane_coord[1].y<=0 || plane_coord[1].y>=pattern_local.rows)
{
plane_coord[1] = cv::Point2i(50, pattern_local.rows-50);
config.setValue("background_plane/x2", plane_coord[1].x);
config.setValue("background_plane/y2", plane_coord[1].y);
}
if (plane_coord[2].x<=0 || plane_coord[2].x>=pattern_local.cols
|| plane_coord[2].y<=0 || plane_coord[2].y>=pattern_local.rows)
{
plane_coord[2] = cv::Point2i(pattern_local.cols-50, 50);
config.setValue("background_plane/x3", plane_coord[2].x);
config.setValue("background_plane/y3", plane_coord[2].y);
}
*/
//init point cloud
int scale_factor = 1;
int out_cols = pattern_image.cols/scale_factor;
int out_rows = pattern_image.rows/scale_factor;
pointcloud.clear();
pointcloud.init_points(out_rows, out_cols);
pointcloud.init_color(out_rows, out_cols);
//progress
QProgressDialog * progress = NULL;
if (parent_widget)
{
progress = new QProgressDialog("Reconstruction in progress.", "Abort", 0, pattern_image.rows, parent_widget,
Qt::Dialog|Qt::CustomizeWindowHint|Qt::WindowCloseButtonHint);
progress->setWindowModality(Qt::WindowModal);
progress->setWindowTitle("Processing");
progress->setMinimumWidth(400);
}
//take 3 points in back plane
/*cv::Mat plane;
if (remove_background)
{
cv::Point3d p[3];
for (unsigned i=0; i<3;i++)
{
for (unsigned j=0;
j<10 && (
INVALID(pattern_local.at<cv::Vec2f>(plane_coord[i].y, plane_coord[i].x)[0])
|| INVALID(pattern_local.at<cv::Vec2f>(plane_coord[i].y, plane_coord[i].x)[1])); j++)
{
plane_coord[i].x += 1.f;
}
const cv::Vec2f & pattern = pattern_local.at<cv::Vec2f>(plane_coord[i].y, plane_coord[i].x);
const float col = pattern[0];
const float row = pattern[1];
if (projector_size.width<=static_cast<int>(col) || projector_size.height<=static_cast<int>(row))
{ //abort
continue;
}
//shoot a ray through the image: u=\lambda*v + q
cv::Point3d u1 = camera.to_world_coord(plane_coord[i].x, plane_coord[i].y);
cv::Point3d v1 = camera.world_ray(plane_coord[i].x, plane_coord[i].y);
//shoot a ray through the projector: u=\lambda*v + q
cv::Point3d u2 = projector.to_world_coord(col, row);
cv::Point3d v2 = projector.world_ray(col, row);
//compute ray-ray approximate intersection
double distance = 0.0;
p[i] = geometry::approximate_ray_intersection(v1, u1, v2, u2, &distance);
std::cout << "Plane point " << i << " distance " << distance << std::endl;
}
plane = geometry::get_plane(p[0], p[1], p[2]);
if (cv::Mat(plane.rowRange(0,3).t()*cv::Mat(cv::Point3d(p[0].x, p[0].y, p[0].z-1.0)) + plane.at<double>(3,0)).at<double>(0,0)
<0.0)
{
plane = -1.0*plane;
}
std::cout << "Background plane: " << plane << std::endl;
}
*/
cv::Mat Rt = calib.R.t();
unsigned good = 0;
unsigned bad = 0;
unsigned invalid = 0;
unsigned repeated = 0;
for (int h=0; h<pattern_image.rows; h+=scale_factor)
{
if (progress && h%4==0)
{
progress->setValue(h);
progress->setLabelText(QString("Reconstruction in progress: %1 good points/%2 bad points").arg(good).arg(bad));
QApplication::instance()->processEvents();
}
if (progress && progress->wasCanceled())
{ //abort
pointcloud.clear();
return;
}
register const cv::Vec2f * curr_pattern_row = pattern_image.ptr<cv::Vec2f>(h);
register const cv::Vec2b * min_max_row = min_max_image.ptr<cv::Vec2b>(h);
for (register int w=0; w<pattern_image.cols; w+=scale_factor)
{
double distance = max_dist; //quality meassure
cv::Point3d p; //reconstructed point
//cv::Point3d normal(0.0, 0.0, 0.0);
const cv::Vec2f & pattern = curr_pattern_row[w];
const cv::Vec2b & min_max = min_max_row[w];
if (sl::INVALID(pattern) || pattern[0]<0.f || pattern[1]<0.f
|| (min_max[1]-min_max[0])<static_cast<int>(threshold))
{ //skip
invalid++;
continue;
}
const float col = pattern[0];
const float row = pattern[1];
if (projector_size.width<=static_cast<int>(col) || projector_size.height<=static_cast<int>(row))
{ //abort
continue;
}
cv::Vec3f & cloud_point = pointcloud.points.at<cv::Vec3f>(h/scale_factor, w/scale_factor);
if (!sl::INVALID(cloud_point[0]))
{ //point already reconstructed!
repeated++;
continue;
}
//standard
cv::Point2d p1(w, h);
cv::Point2d p2(col, row);
triangulate_stereo(calib.cam_K, calib.cam_kc, calib.proj_K, calib.proj_kc, Rt, calib.T, p1, p2, p, &distance);
//save texture coordinates
/*
normal.x = static_cast<float>(w)/static_cast<float>(color_image.cols);
normal.y = static_cast<float>(h)/static_cast<float>(color_image.rows);
normal.z = 0;
*/
if (distance < max_dist)
{ //good point
//evaluate the plane
double d = plane_dist+1;
/*if (remove_background)
{
d = cv::Mat(plane.rowRange(0,3).t()*cv::Mat(p) + plane.at<double>(3,0)).at<double>(0,0);
}*/
if (d>plane_dist)
{ //object point, keep
good++;
cloud_point[0] = p.x;
cloud_point[1] = p.y;
cloud_point[2] = p.z;
//normal
/*cpoint.normal_x = normal.x;
cpoint.normal_y = normal.y;
cpoint.normal_z = normal.z;*/
if (color_image.data)
{
const cv::Vec3b & vec = color_image.at<cv::Vec3b>(h, w);
cv::Vec3b & cloud_color = pointcloud.colors.at<cv::Vec3b>(h/scale_factor, w/scale_factor);
cloud_color[0] = vec[0];
cloud_color[1] = vec[1];
cloud_color[2] = vec[2];
}
}
}
else
{ //skip
bad++;
//std::cout << " d = " << distance << std::endl;
}
} //for each column
} //for each row
if (progress)
{
progress->setValue(pattern_image.rows);
progress->close();
delete progress;
progress = NULL;
}
std::cout << "Reconstructed points[simple]: " << good << " (" << bad << " skipped, " << invalid << " invalid) " << std::endl
<< " - repeated points: " << repeated << " (ignored) " << std::endl;
}
void scan3d::reconstruct_model_patch_center(Pointcloud & pointcloud, CalibrationData const& calib,
cv::Mat const& pattern_image, cv::Mat const& min_max_image, cv::Mat const& color_image,
cv::Size const& projector_size, int threshold, double max_dist, QWidget * parent_widget)
{
if (!pattern_image.data || pattern_image.type()!=CV_32FC2)
{ //pattern not correctly decoded
std::cerr << "[reconstruct_model] ERROR invalid pattern_image\n";
return;
}
if (!min_max_image.data || min_max_image.type()!=CV_8UC2)
{ //pattern not correctly decoded
std::cerr << "[reconstruct_model] ERROR invalid min_max_image\n";
return;
}
if (color_image.data && color_image.type()!=CV_8UC3)
{ //not standard RGB image
std::cerr << "[reconstruct_model] ERROR invalid color_image\n";
return;
}
if (!calib.is_valid())
{ //invalid calibration
return;
}
//parameters
//const unsigned threshold = config.value("main/shadow_threshold", 70).toUInt();
//const double max_dist = config.value("main/max_dist_threshold", 40).toDouble();
//const bool remove_background = config.value("main/remove_background", true).toBool();
//const double plane_dist = config.value("main/plane_dist", 100.0).toDouble();
double plane_dist = 100.0;
/* background removal
cv::Point2i plane_coord[3];
plane_coord[0] = cv::Point2i(config.value("background_plane/x1").toUInt(), config.value("background_plane/y1").toUInt());
plane_coord[1] = cv::Point2i(config.value("background_plane/x2").toUInt(), config.value("background_plane/y2").toUInt());
plane_coord[2] = cv::Point2i(config.value("background_plane/x3").toUInt(), config.value("background_plane/y3").toUInt());
if (plane_coord[0].x<=0 || plane_coord[0].x>=pattern_local.cols
|| plane_coord[0].y<=0 || plane_coord[0].y>=pattern_local.rows)
{
plane_coord[0] = cv::Point2i(50, 50);
config.setValue("background_plane/x1", plane_coord[0].x);
config.setValue("background_plane/y1", plane_coord[0].y);
}
if (plane_coord[1].x<=0 || plane_coord[1].x>=pattern_local.cols
|| plane_coord[1].y<=0 || plane_coord[1].y>=pattern_local.rows)
{
plane_coord[1] = cv::Point2i(50, pattern_local.rows-50);
config.setValue("background_plane/x2", plane_coord[1].x);
config.setValue("background_plane/y2", plane_coord[1].y);
}
if (plane_coord[2].x<=0 || plane_coord[2].x>=pattern_local.cols
|| plane_coord[2].y<=0 || plane_coord[2].y>=pattern_local.rows)
{
plane_coord[2] = cv::Point2i(pattern_local.cols-50, 50);
config.setValue("background_plane/x3", plane_coord[2].x);
config.setValue("background_plane/y3", plane_coord[2].y);
}
*/
//init point cloud
//初始化点云数据为NAN，大小是经过缩放的
int scale_factor_x = 1;
int scale_factor_y = (projector_size.width>projector_size.height ? 1 : 2); //XXX HACK: preserve regular aspect ratio XXX HACK
int out_cols = projector_size.width/scale_factor_x;
int out_rows = projector_size.height/scale_factor_y;
pointcloud.clear();
pointcloud.init_points(out_rows, out_cols);//NAN点：point初始化（pointcloud成员变量）
pointcloud.init_color(out_rows, out_cols);//白色图像：color初始化（pointcloud成员变量）
//progress
//take 3 points in back plane
/*cv::Mat plane;
if (remove_background)
{
cv::Point3d p[3];
for (unsigned i=0; i<3;i++)
{
for (unsigned j=0;
j<10 && (
INVALID(pattern_local.at<cv::Vec2f>(plane_coord[i].y, plane_coord[i].x)[0])
|| INVALID(pattern_local.at<cv::Vec2f>(plane_coord[i].y, plane_coord[i].x)[1])); j++)
{
plane_coord[i].x += 1.f;
}
const cv::Vec2f & pattern = pattern_local.at<cv::Vec2f>(plane_coord[i].y, plane_coord[i].x);
const float col = pattern[0];
const float row = pattern[1];
if (projector_size.width<=static_cast<int>(col) || projector_size.height<=static_cast<int>(row))
{ //abort
continue;
}
//shoot a ray through the image: u=\lambda*v + q
cv::Point3d u1 = camera.to_world_coord(plane_coord[i].x, plane_coord[i].y);
cv::Point3d v1 = camera.world_ray(plane_coord[i].x, plane_coord[i].y);
//shoot a ray through the projector: u=\lambda*v + q
cv::Point3d u2 = projector.to_world_coord(col, row);
cv::Point3d v2 = projector.world_ray(col, row);
//compute ray-ray approximate intersection
double distance = 0.0;
p[i] = geometry::approximate_ray_intersection(v1, u1, v2, u2, &distance);
std::cout << "Plane point " << i << " distance " << distance << std::endl;
}
plane = geometry::get_plane(p[0], p[1], p[2]);
if (cv::Mat(plane.rowRange(0,3).t()*cv::Mat(cv::Point3d(p[0].x, p[0].y, p[0].z-1.0)) + plane.at<double>(3,0)).at<double>(0,0)
<0.0)
{
plane = -1.0*plane;
}
std::cout << "Background plane: " << plane << std::endl;
}
*/
//candidate points
QMap<unsigned, cv::Point2f> proj_points;
QMap<unsigned, std::vector<cv::Point2f> > cam_points;
//cv::Mat proj_image = cv::Mat::zeros(out_rows, out_cols, CV_8UC3);
unsigned good = 0;
unsigned bad = 0;
unsigned invalid = 0;
unsigned repeated = 0;
for (int h=0; h<pattern_image.rows; h++)
{
register const cv::Vec2f * curr_pattern_row = pattern_image.ptr<cv::Vec2f>(h);
register const cv::Vec2b * min_max_row = min_max_image.ptr<cv::Vec2b>(h);
for (register int w=0; w<pattern_image.cols; w++)
{
const cv::Vec2f & pattern = curr_pattern_row[w];
const cv::Vec2b & min_max = min_max_row[w];
if (sl::INVALID(pattern)
|| pattern[0]<0.f || pattern[0]>=projector_size.width || pattern[1]<0.f || pattern[1]>=projector_size.height
|| (min_max[1]-min_max[0])<static_cast<int>(threshold))
{ //skip
continue;
}
//ok
cv::Point2f proj_point(pattern[0]/scale_factor_x, pattern[1]/scale_factor_y);
unsigned index = static_cast<unsigned>(proj_point.y)*out_cols + static_cast<unsigned>(proj_point.x);//索引是投影图像上所有的点（uncertain）
proj_points.insert(index, proj_point);
//std::cout<<"index: "<<index<<std::endl;
cam_points[index].push_back(cv::Point2f(w, h));
//std::cout<<"cam_point: "<<cam_points[index]<<std::endl;
//proj_image.at<cv::Vec3b>(static_cast<unsigned>(proj_point.y), static_cast<unsigned>(proj_point.x)) = color_image.at<cv::Vec3b>(h, w);
}
}
//cv::imwrite("proj_image.png", proj_image);
cv::Mat Rt = calib.R.t();
QMapIterator<unsigned, cv::Point2f> iter1(proj_points);
unsigned n = 0;
while (iter1.hasNext())
{
n++;
iter1.next();
unsigned index = iter1.key();
const cv::Point2f & proj_point = iter1.value();
const std::vector<cv::Point2f> & cam_point_list = cam_points.value(index);
const unsigned count = static_cast<int>(cam_point_list.size());
if (!count)
{ //empty list
continue;
}
//center average
cv::Point2d sum(0.0, 0.0), sum2(0.0, 0.0);
for (std::vector<cv::Point2f>::const_iterator iter2=cam_point_list.begin(); iter2!=cam_point_list.end(); iter2++)
{
sum.x += iter2->x;
sum.y += iter2->y;
sum2.x += (iter2->x)*(iter2->x);
sum2.y += (iter2->y)*(iter2->y);
}
cv::Point2d cam(sum.x/count, sum.y/count);//
cv::Point2d proj(proj_point.x*scale_factor_x, proj_point.y*scale_factor_y);
//triangulate
double distance = max_dist; //quality meassure
cv::Point3d p; //reconstructed point
triangulate_stereo(calib.cam_K, calib.cam_kc, calib.proj_K, calib.proj_kc, Rt, calib.T, cam, proj, p, &distance);
if (distance < max_dist)
{ //good point
//evaluate the plane
double d = plane_dist+1;
/*if (remove_background)
{
d = cv::Mat(plane.rowRange(0,3).t()*cv::Mat(p) + plane.at<double>(3,0)).at<double>(0,0);
}*/
if (d>plane_dist)
{ //object point, keep
good++;
cv::Vec3f & cloud_point = pointcloud.points.at<cv::Vec3f>(proj_point.y, proj_point.x);
cloud_point[0] = p.x;
cloud_point[1] = p.y;
cloud_point[2] = p.z;
//std::cout << " pointcloud.points= " <<cloud_point << std::endl;
if (color_image.data)//每個像素點對應的彩色值，存入
{
const cv::Vec3b & vec = color_image.at<cv::Vec3b>(static_cast<unsigned>(cam.y), static_cast<unsigned>(cam.x));
cv::Vec3b & cloud_color = pointcloud.colors.at<cv::Vec3b>(proj_point.y, proj_point.x);
cloud_color[0] = vec[0];
cloud_color[1] = vec[1];
cloud_color[2] = vec[2];
}
}
}
else
{ //skip
bad++;
//std::cout << " d = " << distance << std::endl;
}
} //while
std::cout << "Reconstructed points [patch center]: " << good << " (" << bad << " skipped, " << invalid << " invalid) " << std::endl
<< " - repeated points: " << repeated << " (ignored) " << std::endl;
}
int Test::run(){
std::cout << "Enviroment Simulator test begin" << std::endl;
int error = 0;
int size = 20;
std::ofstream testResultsFile;
testResultsFile.open("testResult.txt");
if(!testResultsFile.is_open()){
testResultsFile << "Error: ";
std::cout << "Result log: " << "Log file could not be opend" << std::endl;
++error;
}
testResultsFile << "Enviroment Simulator test begin" << std::endl;
//Map not from file
testResultsFile << "Enviroment Simulator test Map not from file" << std::endl;
Map testMap("testMap.map", size,size);
if(testMap.height != size || testMap.width != size){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Map y size incorret" << std::endl;
++error;
}
if(testMap.height != size || testMap.width != size){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Map x size incorret" << std::endl;
++error;
}
for(int y = 0; y < testMap.height; y++){
for(int x = 0; x < testMap.width; x++){
if(testMap.getMapObject(y,x) != 0){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Conntent error. != 0" << std::endl;
++error;
}
testMap.setMapObject(1,y,x);
}
}
for(int y = 0; y < testMap.height; y++){
for(int x = 0; x < testMap.width; x++){
if(testMap.getMapObject(y,x) != 1){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Conntent error. != 1" << std::endl;
++error;
}
}
}
testMap.setMapObject(0,size - 5,size - 2);
if(testMap.getMapObject(size - 5,size - 2) != 0){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Object error size - 5,size - 2 != 0" << std::endl;
++error;
}
testMap.setMapObject(1,size - size,size - size);
if(testMap.getMapObject(size - size,size -size) != 1){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Object error 0,0 != 1" << std::endl;
++error;
}
testMap.setMapObject(1,size - 1,size - (size - 1));
if(testMap.getMapObject(size - 1,size - (size - 1)) != 1){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Object error size - 1,size - (size - 1) != 1" << std::endl;
++error;
}
testMap.setMapObject(0,size - 1,size - 1);
if(testMap.getMapObject(size - 1,size - 1) != 0){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Object error size - 1,size - (size - 1) != 0" << std::endl;
++error;
}
testMap.setMapObject(0,size + 1,size + 1);
if(testMap.getMapObject(size + 1,size + 1) != -1){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Object error size - 1,size - (size - 1) != -1" << std::endl;
++error;
}
testMap.setMapObject(0,size,size);
if(testMap.getMapObject(size,size) != -1){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Object error size, size != -1" << std::endl;
++error;
}
testMap.setMapObject(0,size - 21,size - 21);
if(testMap.getMapObject(size - 21,size - 21) != -1){
testResultsFile << "Error: ";
testResultsFile << "Map not from file: " << "Object error size - 21,size - 21 + 1 != 1" << std::endl;
++error;
}
for(int y = 0; y < testMap.height; y++){
for(int x = 0; x < testMap.width; x++){
testMap.setMapObject(0,y,x);
}
}
//Save file
testResultsFile << "Enviroment Simulator test Save file" << std::endl;
testMap.saveMap("testFileMap.map");
//Map from file
testResultsFile << "Enviroment Simulator test Map from file" << std::endl;
Map testFileMap("testFileMap.map");
try{
Map testFileMap("testFileMap.map");
}
catch(int e){
if(e != 2){
testResultsFile << "Error: ";
testResultsFile << "Map from corrupt file: " << "Excpention error." << std::endl;
++error;
}
}
if(testFileMap.width != size || testFileMap.height != size){
testResultsFile << "Error: ";
testResultsFile << "Map from file: " << "Map y size incorret" << std::endl;
++error;
}
if(testFileMap.width != size || testFileMap.height != size){
testResultsFile << "Error: ";
testResultsFile << "Map from file: " << "Map x size incorret" << std::endl;
++error;
}
for(int y = 0; y < testFileMap.height; y++){
for(int x = 0; x < testFileMap.width; x++){
if(testFileMap.getMapObject(y,x) != 0){
testResultsFile << "Error: ";
testResultsFile << "Map from file: " << "Conntent error. != 0" << std::endl;
++error;
}
}
}
for(int y = 0; y < testFileMap.height; y++){
for(int x = 0; x < testFileMap.width; x++){
testFileMap.setMapObject(1,y,x);
}
}
for(int y = 0; y < testFileMap.height; y++){
for(int x = 0; x < testFileMap.width; x++){
if(testFileMap.getMapObject(y,x) != 1){
testResultsFile << "Error: ";
testResultsFile << "Map from file: " << "Conntent error. != 1" << std::endl;
++error;
}
}
}
//Map from corrupt file
testResultsFile << "Enviroment Simulator test Map from corrupt file" << std::endl;
try{
Map corruptMap("testFileMapCorrupt.map");
++error;
}
catch(int e){
if(e != 2){
testResultsFile << "Error: ";
testResultsFile << "Map from corrupt file: " << "Excpention error." << std::endl;
}
}
for(int y = 0; y < testMap.height; y++){
for(int x = 0; x < testMap.width; x++){
testMap.setMapObject(0,y,x);
}
}
//Simuate test
testResultsFile << "Enviroment Simulator test Simulate" << std::endl;
Map *tp = new Map("testSimulateMap.map" , size, size);
tp->setMapObject(1,1,9);
tp->setMapObject(1,15,15);
SimulateMap testSim(tp);
testSim.addCheckPoint(6,2);
testSim.addCheckPoint(21,21);
testResultsFile << "addCheckPoint 21 21" << std::endl;
testSim.addCheckPoint(-1,-1);
testResultsFile << "addCheckPoint -1 -1" << std::endl;
testSim.simulate();
testResultsFile << "Simulate: " << "Lest see" << std::endl;
Map *pt = new Map("temp.map", size, size);
SimulateMap testSimEmpty(pt);
testSimEmpty.simulate();
testResultsFile << "Simulate: " << "Lest see" << std::endl;
testSimEmpty.getPointCloud();
testResultsFile << "Simulate: " << "Lest see" << std::endl;
//Objects behind objects test
Map *tM = new Map("testMap.map", size,size);
//right side
tM->setMapObject(1,5,8);
testResultsFile << "addObject 5 8 right side from checkpoint" << std::endl;
tM->setMapObject(1,5,9);
testResultsFile << "addObject 5 9 right side from checkpoint" << std::endl;
//left side
tM->setMapObject(1,5,6);
testResultsFile << "addObject 5 6 left side from checkpoint" << std::endl;
tM->setMapObject(1,5,5);
testResultsFile << "addObject 5 5 left side from checkpoint" << std::endl;
//upper side
tM->setMapObject(1,4,7);
testResultsFile << "addObject 4 7 upper side from checkpoint" << std::endl;
tM->setMapObject(1,3,7);
testResultsFile << "addObject 3 7 upper side from checkpoint" << std::endl;
//lower side
tM->setMapObject(1,6,7);
testResultsFile << "addObject 7 6 lower side from checkpoint" << std::endl;
tM->setMapObject(1,7,7);
testResultsFile << "addObject 7 7 lower side from checkpoint" << std::endl;
//left up side diagonal
tM->setMapObject(1,4,6);
testResultsFile << "addObject 4 6 left up side diagonal checkpoint" << std::endl;
tM->setMapObject(1,3,5);
testResultsFile << "addObject 5 3 left up side diagonal checkpoint" << std::endl;
//left down side diagonal
tM->setMapObject(1,6,6);
testResultsFile << "addObject 6 6 left down side diagonal checkpoint" << std::endl;
tM->setMapObject(1,7,5);
testResultsFile << "addObject 5 7 left down side diagonal checkpoint" << std::endl;
//right up side diagonal
tM->setMapObject(1,4,8);
testResultsFile << "addObject 8 4 right up side diagonal checkpoint" << std::endl;
tM->setMapObject(1,3,9);
testResultsFile << "addObject 9 3 right up side diagonal checkpoint" << std::endl;
//right down side diagonal
tM->setMapObject(1,6,8);
testResultsFile << "addObject 8 6 right down side diagonal checkpoint" << std::endl;
tM->setMapObject(1,7,9);
testResultsFile << "addObject 9 7 right down side diagonal checkpoint" << std::endl;
SimulateMap tS(tM);
tS.addCheckPoint(7,5);
testResultsFile << "addCheckPoint 5 7" << std::endl;
tS.simulate();
testResultsFile << "Enviroment Simulator object behind object test PointCloud" << std::endl;
Pointcloud pointc = tS.getPointCloud();
for(Pointcloud::Point p : *pointc.getPoints()){
if(p.X == 2 && p.Y == 0){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Right side object behind object scanned. FAILURE!" << std::endl;
++error;
}
else if(p.X == -2 && p.Y == 0){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Left side object behind object scanned. FAILURE!" << std::endl;
++error;
}
else if(p.X == 0 && p.Y == 2){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Upper side object behind object scanned. FAILURE!" << std::endl;
++error;
}
else if(p.X == 0 && p.Y == -2){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Lower side object behind object scanned. FAILURE!" << std::endl;
++error;
}
else if(p.X == -2 && p.Y == -2){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Upper left diagonal side object behind object scanned. FAILURE!" << std::endl;
++error;
}
else if(p.X == 2 && p.Y == -2){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Lower left diagonal side object behind object scanned. FAILURE!" << std::endl;
++error;
}
else if(p.X == 2 && p.Y == 2){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Lower right diagonal side object behind object scanned. FAILURE!" << std::endl;
++error;
}
else if(p.X == -2 && p.Y == 2){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Upper right diagonal side object behind object scanned. FAILURE!" << std::endl;
++error;
}
}
//PointCloud test
testResultsFile << "Enviroment Simulator test PointCloud" << std::endl;
Pointcloud pC = testSim.getPointCloud();
if(pC.getPoints()->size() > 1){
testResultsFile << "Error: ";
++error;
testResultsFile << "PointCloud: " << "Radius failed" << std::endl;
}
for(Pointcloud::Point p : *pC.getPoints()){
//std::cout << "x: " << p.X << " y: " << p.Y;
if(p.X != 3){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "X wrong" << std::endl;
++error;
}
if(p.Y != -1){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Y wrong" << std::endl;
++error;
}
}
//save pc
pC.savePointsToFile("testpC.pcl");
Pointcloud loadpC;
//load pc from file
loadpC.loadPointsFromFile("testpC.pcl");
//make map from loaded pc
try{
Map pCloadMap("testpC.map", &loadpC);
//check point
if(pCloadMap.getMapObject(0,3) != 1){
testResultsFile << "PointCloud: " << "Load from file failed" << std::endl;
}
}
catch(int e){
if(e != 2){
testResultsFile << "Error: ";
testResultsFile << "PointCloud: " << "Excpention error." << std::endl;
++error;
}
}
//Done
testResultsFile << "Enviroment Simulator test done" << std::endl;
if(error == 0){
testResultsFile << "Test Succes!" << std::endl;
std::cout << "Test Succes!" << std::endl;
}
testResultsFile << '\n' << '\n';
testResultsFile.close();
std::cout << "Enviroment Simulator test done" << std::endl;
return error;
}