C++ OcTree::insertRay方法代码示例

OcTree* generateSphereTree(point3d origin, float radius){
	OcTree* tree = new OcTree(0.05);

	point3d point_on_surface = origin;
	point_on_surface.x() += radius;
	for (int i=0; i<360; i++) {
		for (int j=0; j<360; j++) {
			if (!tree->insertRay(origin, origin+point_on_surface)) {
				cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
			}
			point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
		}
		point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
	}
	return tree;
}

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

int main(int argc, char** argv) {


  //##############################################################     

  OcTree tree (0.05);  

  //  point3d origin (10.01, 10.01, 10.02);
  point3d origin (0.01f, 0.01f, 0.02f);
  point3d point_on_surface (2.01f, 0.01f, 0.01f);

  cout << "generating sphere at " << origin << " ..." << endl;

  for (int i=0; i<360; i++) {    
    for (int j=0; j<360; j++) {
      if (!tree.insertRay(origin, origin+point_on_surface)) {
        cout << "ERROR while inserting ray from " << origin << " to " << point_on_surface << endl;
      }
      point_on_surface.rotate_IP (0,0,DEG2RAD(1.));
    }
    point_on_surface.rotate_IP (0,DEG2RAD(1.),0);
  }  

  cout << "done." << endl;
  cout << "writing to sphere.bt..." << endl;
  tree.writeBinary("sphere.bt");

  // -----------------------------------------------

  cout << "casting rays ..." << endl;

  OcTree sampled_surface (0.05);  

  point3d direction = point3d (1.0,0.0,0.0);
  point3d obstacle(0,0,0);

  unsigned int hit (0);
  unsigned int miss (0);
  double mean_dist(0);

  for (int i=0; i<360; i++) {    
    for (int j=0; j<360; j++) {
      if (!tree.castRay(origin, direction, obstacle, true, 3.)) {
        miss++;
      }
      else {
        hit++;
        mean_dist += (obstacle - origin).norm();
        sampled_surface.updateNode(obstacle, true);
      }
      direction.rotate_IP (0,0,DEG2RAD(1.));
    }
    direction.rotate_IP (0,DEG2RAD(1.),0);
  }
  cout << "done." << endl;

  mean_dist /= (double) hit;
  std::cout << " hits / misses: " << hit  << " / " << miss << std::endl;
  std::cout << " mean obstacle dist: " << mean_dist << std::endl;

  cout << "writing sampled_surface.bt" << endl;
  sampled_surface.writeBinary("sampled_surface.bt");

  // -----------------------------------------------

  cout << "generating single rays..." << endl;
  OcTree single_beams(0.03333);
  int num_beams = 17;
  float beamLength = 10.0f;
  point3d single_origin (1.0f, 0.45f, 0.45f);
  point3d single_endpoint(beamLength, 0.0f, 0.0f);
	
	
  for (int i=0; i<num_beams; i++) {    
    if (!single_beams.insertRay(single_origin, single_origin+single_endpoint)) {
      cout << "ERROR while inserting ray from " << single_origin << " to " << single_endpoint << endl;
    }
    single_endpoint.rotate_IP (0,0,DEG2RAD(360.0/num_beams));
  }
	
  cout << "done." << endl;
  cout << "writing to beams.bt..." << endl;
  single_beams.writeBinary("beams.bt");

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