C++ Pointcloud类代码示例

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







}

  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
 }

//.........这里部分代码省略.........
      }
    } 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++;
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    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);

 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
//.........这里部分代码省略.........

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);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
    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;
        }