BOOST_FOREACH(boost::shared_ptr<count_writer_t> p_count_writer, mCellPopulationCountWriters)
 {
     p_count_writer->OpenOutputFile(rOutputFileHandler);
     p_count_writer->WriteHeader(this);
 }
inline std::vector<int> const &ProblemData::getSamplesClasses() const
{
    return *ptrSamplesClasses.get();
}
void ribi::gtst::ParticipantDialogStateChooseAction::RespondToTimedServerPush()
{
  const int time_left = m_server->GetStates()->GetCurrentState()->GetTimeLeft();

  //Update label_time_left
  {
    std::string text
    =  std::string("Time left: ")
    + std::to_string(time_left)
    + std::string(" seconds");
    if (m_server->GetParameters()->GetChooseAction()->GetWait())
    {
      //Infinite time
      text+=std::string(" (waiting for others)");
    }
    assert(ui.m_label_time_left);
    ui.m_label_time_left->setText(text.c_str());
  }

  //Check if choice must be sent to the server
  if (!m_server->GetParameters()->GetChooseAction()->GetWait()
    && time_left <= 0)
  {
    assert(GetDialog());
    assert(GetDialog()->CanGetParticipant());
    if (ui.m_button_choose_action->isEnabled())
    {
      //Let Participant choose a random action by time
      const int selected = ui.m_group->selectedButtonIndex();
      assert(ui.m_group->count() > 0);
      const int action_index
        = (selected == -1 //Did user select something?
        ? std::rand() % ui.m_group->count() //Take a random action
        : selected);
      assert(action_index > -1 && action_index < ui.m_group->count());
      ui.m_group->setSelectedButtonIndex(action_index);
      OnChooseActionClick();
    }
  }


  if (!ui.m_button_choose_action->isEnabled())
  {
    assert(ui.m_group->selectedButtonIndex() != -1
      && "m_button_choose_action should only disable when an action is selected");

    const std::vector<boost::shared_ptr<ChooseActionOption> >& options
      = m_server->GetParameters()->GetChooseAction()->GetOptions();

    assert(ui.m_group->selectedButtonIndex() < static_cast<int>(options.size()));

    const boost::shared_ptr<ChooseActionOption> option
      = options[ ui.m_group->selectedButtonIndex() ];

    const std::string text
      = option->GetMessageChoice()
      + std::string("\', waiting for the others...");

    ui.m_label_status->setText(text.c_str());
  }
  else
  {
    ui.m_label_status->setText("Please choose an action.");
  }

  ///Follow the server its tempo
  RespondToParticipant();
}
Exemple #4
0
 template<typename F, typename G> locked_ptr(boost::shared_ptr<T> pointer, F lock, G unlock):
     boost::shared_ptr<T>(pointer.get(), unlock),
     value(pointer)
 {
     lock();
 };
inline boost::unordered_map<std::string, int> const &ProblemData::getClassMap() const
{
    return *ptrClassMap.get();
}
void ribi::TriangleMeshCreatorMenuDialog::TestDeep() noexcept
{
  const bool verbose{false};

  if (verbose) { TRACE("Trying out to build cells from the hardest testing templates"); }
  {
    using ribi::trim::Cell;
    using ribi::trim::CellsCreator;
    using ribi::trim::CellsCreatorFactory;
    using ribi::trim::CreateVerticalFacesStrategy;
    using ribi::trim::CreateVerticalFacesStrategies;
    using ribi::trim::Dialog;
    using ribi::trim::Template;
    //This is the longest test by far
    //const TestTimer test_timer(boost::lexical_cast<std::string>(__LINE__),__FILE__,1.0);
    for (CreateVerticalFacesStrategy strategy: CreateVerticalFacesStrategies().GetAll())
    {
      const boost::shared_ptr<Template> my_template {
        Template::CreateTest(3)
      };

      const int n_cell_layers = 2;
      const boost::shared_ptr<CellsCreator> cells_creator{
        CellsCreatorFactory().Create(
          my_template,
          n_cell_layers,
          1.0 * boost::units::si::meter,
          strategy,
          verbose
        )
      };
      const std::vector<boost::shared_ptr<Cell>> cells { cells_creator->GetCells() };
      assert(cells.size() > 0);
    }
  }

  if (verbose) { TRACE("Testing case 1"); }
  TriangleMeshCreatorMenuDialog().Execute(
    {
      "TriangleMeshCreator",
      "--layer_height", "1",
      "--WKT", "POLYGON((1 1,-1 1,-1 -1,1 -1))",
      "--strategy", "1",
      "--n_layers", "1",
      "--fraction", "0.75",
      //"--show_mesh",
      //"--verbose",
      "--triangle_max_area", "10.0",
      "--triangle_min_angle", "20.0",
      "--profile"
    }
  );
  if (verbose) { TRACE("Testing case 2"); }
  TriangleMeshCreatorMenuDialog().Execute(
    {
      "TriangleMeshCreator",
      "-z", "1",
      "-w", "POLYGON((0 0,0 3,3 0)),POLYGON((1 1,0 2,2 0))",
      "-s", "1",
      "-n", "1",
      "-f", "0.75",
      //"-m",
      //"-b",
      "--triangle_area", "10.0",
      "--triangle_quality", "20.0",
      "--profile"
    }
  );
  if (verbose) { TRACE("Testing case 3"); }
  TriangleMeshCreatorMenuDialog().Execute(
    {
      "TriangleMeshCreator",
      "-z", "1",
      "-w", "POLYGON((10 10,10 -10,-10 -10,-10 10)),LINESTRING(5 5,5 -5,-5 -5,-5 5)",
      "-s", "1",
      "-n", "1",
      "-f", "0.75",
      //"-m",
      //"-b",
      "-r", "10.0",
      "-q", "20.0",
      "--profile"
    }
  );
  if (verbose) { TRACE("Testing case 4"); }
  TriangleMeshCreatorMenuDialog().Execute(
    {
      "TriangleMeshCreator",
      "-z", "1",
      "-w", "LINESTRING(5 5,5 -5,-5 -5,-5 5)",
      "-s", "1",
      "-n", "1",
      "-f", "0.75",
      //"-m",
      //"-b",
      "-r", "10.0",
      "-q", "20.0",
      "--profile"
    }
  );
  if (verbose) { TRACE("Testing mesh of issue #221"); }

  TriangleMeshCreatorMenuDialog().Execute(
    {
      "TriangleMeshCreator",
      "--layer_height","1",
      "--WKT", "POLYGON((10 10,10 -10,-10 -10,-10 10))",
      "--strategy", "1",
      "--n_layers", "10",
      "--fraction", "0.9",
      "--triangle_max_area", "0.1",
      "--triangle_min_angle", "20.0",
      "--show_mesh",
      "--profile"
    }
  );
  if (verbose) { TRACE("Tested all cases"); }
}
 ///////////////////////////////////////////////////////////////////
 ///Syncronize objects  only for clouds
 //////////////////////////////////////////////////////////////////
 void Callback(const sensor_msgs::PointCloud2ConstPtr& cloudI)
 {
  
   
   ROS_INFO("cloud timestamps %i.%i ", cloudI->header.stamp.sec,cloudI->header.stamp.nsec);
      
   //////////////////////////////////////////////////////////////////
   ///for range images
   /////////////////////////////////////////////////////////////////
   
   Eigen::Affine3f sensorPose = (Eigen::Affine3f)Eigen::Translation3f(0.0f, 0.0f, 0.0f);
   
   pcl::RangeImage::CoordinateFrame coordinate_frame = pcl::RangeImage::LASER_FRAME;
   
   float noiseLevel=0.00;
   
   float minRange = 0.0f;
   
   int borderSize = 1;
   
   
   /////////////////////////////////////////////////////////////////////
   /// point cloud 
   ///////////////////////////////////////////////////////////////////
   PointCloud<PointXYZ>::Ptr cloud_ptr (new PointCloud<PointXYZ>);
   
   
   if ((cloudI->width * cloudI->height) == 0)
     return ;
   
   ROS_INFO ("Received %d data points in frame %s with the following fields: %s",
	     
	     (int)cloudI->width * cloudI->height,
	     
	     cloudI->header.frame_id.c_str (),
	     
	     pcl::getFieldsList (*cloudI).c_str ());
   
   
   
   
   pcl::fromROSMsg(*cloudI, *cloud_ptr);

     
   ///segment data only the part in front of
   SegmentData(cloud_ptr,cloud_filtered,cloud_nonPlanes,cloud_Plane,cloud_projected);
    
   ///////////////////////////////////////////////
   ///image range images
   //////////////////////////////////////////////
   range_image->createFromPointCloud (*cloud_filtered, pcl::deg2rad(0.2f),
				     pcl::deg2rad (360.f), pcl::deg2rad (180.0f),
				     sensorPose, coordinate_frame, noiseLevel, minRange, borderSize);
   
   /////////////////////////////////////////////////////////////////////////////////////////////
   ///planar range image TESTING
   //////////////////////////////////////////////////////////////////////////////////////////
   
   Eigen::Affine3f trans,P_affine;//does not include K
   

     trans (0,0)= 0.0f; trans (0,1)= 0.0f; trans (0,2)=-1.0f; trans(0,3)=0.0f;       

     trans (1,0)= 0.0f; trans (1,1)= 1.0f; trans (1,2)=0.0f; trans (1,3)=0.0f;

     trans (2,0)= 1.0f; trans (2,1)= 0.0f; trans (2,2)=0.0f; trans (2,3)=0.0f;

     trans (3,0)= 0.0f; trans (3,1)= 0.0f; trans (3,2)=0.0f; trans (3,3)=1.0f;
     
     
     P_affine (0,0)= 0.0187f; P_affine (0,1)= -0.8024f; P_affine (0,2)= -0.5965f; P_affine(0,3)=-0.7813f;       

     P_affine (1,0)= -0.9998f; P_affine (1,1)= -0.0168f; P_affine (1,2)=-0.0088f; P_affine (1,3)=0.1818f;

     P_affine (2,0)= -0.0030f; P_affine (2,1)= 0.5965f; P_affine (2,2)=-0.8026f; P_affine (2,3)=0.7670f;

     P_affine (3,0)= 0.0f; P_affine (3,1)= 0.0f; P_affine (3,2)=0.0f; P_affine (3,3)=1.0f;
     
     
//    0.0187   -0.8024   -0.5965   -0.7813
//    -0.9998   -0.0168   -0.0088    0.1818
//    -0.0030    0.5965   -0.8026    0.7670

   for(unsigned int i=0; i<cloud_filtered->size();i++)
   {
      PointXYZ p1,p2;
      p1=cloud_filtered->points[i];
      
      p2.x=-p1.z;
      p2.y=p1.y;
      p2.z=p1.x;
      
      cloud_trans->push_back(p2);
   }
     
  // pcl::transformPointCloud (*cloud_filtered, *cloud_filtered, trans);
   Eigen::Affine3f pose(Eigen::Affine3f::Identity());
  // pose=pose*Translation3f(T_);
   pose=P_affine;
   ///
   range_image_planar->createFromPointCloudWithFixedSize(*cloud_filtered,768, 1024,
			//K_(0,2),K_(1,2),K_(0,0),K_(1,1),P_affine
			K_(0,2),K_(1,2),K_(0,0),K_(1,1),  Eigen::Affine3f::Identity()
                             ,pcl::RangeImage::LASER_FRAME, noiseLevel, minRange);
 
   //GenerarImagenOpenCV(range_image,im_range_current, im_range_current_norm);
   //TESTING PART with planar range image
  GenerarImagenOpenCV(range_image_planar,im_range_current, im_range_current_norm);

  applyColorMap( im_range_current_norm, falseColorsMap, COLORMAP_HOT);


   if(!firstFrame)

   {
    Mat im2 = Mat(im_range_previous_norm.rows, im_range_previous_norm.cols, im_range_previous_norm.type());
    cv::resize(im_range_current_norm, im2,im2.size(), INTER_LINEAR );
    cv::calcOpticalFlowFarneback(im2,im_range_previous_norm,flow,0.5, 3, 15, 3, 5, 1.2, 0);
   
    ///show data

    cv::cvtColor( im2 ,cflow, CV_GRAY2BGR);
   
    drawOptFlowMap(flow, cflow, 10, 1.5, Scalar(0, 255, 0));
    drawOpticalFlow_color(flow,color_flow, -1);
  
   
    
     
    //Set_range_flow(range_image,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
    ///planar range image TESTING
    Set_range_flow(range_image_planar,im2,flow,cloud_flow,cloud_p3d,normals,cloud_rgb);
  
   ////////////////////////////////////////////////////////////////////////////////////
    ///publishing
    //////////////////////////////////////////////////////////////////////////////////
  
    cv_bridge::CvImagePtr  cv_send(new cv_bridge::CvImage);

    cv_send->image=Mat(falseColorsMap.rows, falseColorsMap.cols, CV_8UC3);
    cv_send->encoding=enc::RGB8;

   ///range image 

    falseColorsMap.copyTo(cv_send->image);

    pub_im_range.publish(cv_send->toImageMsg());


   //flow arrows

   
   cv_send->image=Mat(cflow.rows, cflow.cols, CV_8UC3);
   cflow.copyTo(cv_send->image);
   pub_im_range_flow.publish(cv_send->toImageMsg());

   
   ///color flow

   cv_send->image=Mat( color_flow.rows,  color_flow.cols, CV_8UC3);
   color_flow.copyTo(cv_send->image);

     pub_im_range_color_flow.publish(cv_send->toImageMsg());

     
   ///cloud flow
    sensor_msgs::PointCloud2 output_cloud;
   pcl::toROSMsg(*cloud_rgb,output_cloud);
   output_cloud.header.frame_id = cloudI->header.frame_id;
   pub_cloud_flow.publish(output_cloud);

   
    ////////////////////////////////////
   ///cloud complete
   ///////////////////////////////////////
   pub_cloud.publish(cloudI);
    ////////////////////////////////////////////
   ///point normal
   /////////////////////////////////////////////
   sensor_msgs::PointCloud2 output_cloud2;  
   pcl::concatenateFields(*cloud_p3d,* normals,*cloud_normals);

   

   pcl::toROSMsg(*cloud_normals,output_cloud2);
   output_cloud2.header.frame_id = cloudI->header.frame_id;
   pub_cloud_normal.publish(output_cloud2);
   
   //////////////////////////////////////////////////////////////
   ///markers
   ///////////////////////////////////////////////////////////
   visualization_msgs::Marker points,line_list;
   Publish_Marks(cloud_flow,cloud_p3d, points,line_list);
   pub_markers.publish(points);
   pub_markers.publish(line_list);
   
  }
  
    

  //create the images
  firstFrame=0;// we have read the first frame

  im_range_current_norm.copyTo(im_range_previous_norm);
  flow=Mat(im_range_current_norm.rows, im_range_current_norm.cols,CV_32FC2);
  cflow=Mat(im_range_current_norm.rows, im_range_current_norm.cols, CV_8UC3);
  
  
  
    
 }
Exemple #8
0
void VectorPort::onChildCreate(
	const boost::shared_ptr<BaseVectorType::ChildType>& inCreatedChild) const throw (Error) {
	inCreatedChild->setName(getName());
}
Connection::Connection(boost::shared_ptr< Hive > hive)
	: m_hive(hive), m_socket(hive->GetService()), m_io_strand(hive->GetService()), m_timer(hive->GetService()), m_receive_buffer_size(4096), m_timer_interval(1000), m_error_state(0)
{
}
  OverlayPluginTestFixture()
  {
    arrayPool = new NDArrayPool(100, 0);
    expectedArrayCounter=0;

    // Asyn manager doesn't like it if we try to reuse the same port name for multiple drivers
    // (even if only one is ever instantiated at once), so we change it slightly for each test case.
    std::string simport("simOVER1"), testport("OVER1");
    uniqueAsynPortName(simport);
    uniqueAsynPortName(testport);

    // We need some upstream driver for our test plugin so that calls to connectArrayPort
    // don't fail, but we can then ignore it and send arrays by calling processCallbacks directly.
    driver = boost::shared_ptr<asynPortDriver>(new asynPortDriver(simport.c_str(),
                                                                     1, 1,
                                                                     asynGenericPointerMask,
                                                                     asynGenericPointerMask,
                                                                     0, 0, 0, 2000000));

    // This is the plugin under test
    Overlay = boost::shared_ptr<OverlayPluginWrapper>(new OverlayPluginWrapper(testport.c_str(),
                                                                      50,
                                                                      1,
                                                                      simport.c_str(),
                                                                      0,
                                                                      8,
                                                                      0,
                                                                      0,
                                                                      0,
                                                                      1));
    // This is the mock downstream plugin
    downstream_plugin = new TestingPlugin(testport.c_str(), 0);

    // Enable the plugin
    Overlay->start(); // start the plugin thread although not required for this unittesting
    Overlay->write(NDPluginDriverEnableCallbacksString, 1);
    Overlay->write(NDPluginDriverBlockingCallbacksString, 1);

    client = boost::shared_ptr<asynGenericPointerClient>(new asynGenericPointerClient(testport.c_str(), 0, NDArrayDataString));
    client->registerInterruptUser(&Overlay_callback);

    // Test a "normal" case
    overlayTempCaseStr test1 = {0, 500, 500, 50, 50, 1, 1, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test1); 
    // Test a case with size larger than array
    overlayTempCaseStr test2 = {0, 500, 500, 5000, 5000, 1, 1, 0, 255, 0, NDOverlayEllipse, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test2); 
    // Test a case with zero size
    overlayTempCaseStr test3 = {0, 500, 500, 0, 0, 1, 1, 0, 255, 0, NDOverlayRectangle, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test3); 
    // Test a case with negative position
    overlayTempCaseStr test4 = {0, -500, -500, 50, 50, 1, 1, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test4); 
    // Test a case with position larger than array size
    overlayTempCaseStr test5 = {0, 1500, 1500, 50, 50, 1, 1, 0, 255, 0, NDOverlayEllipse, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test5); 
    // Test an RGB1 case 
    overlayTempCaseStr test6 = {0, 500, 500, 50, 50, 1, 1, 0, 255, 0, NDOverlayRectangle, NDOverlaySet, 3, {3, 1024, 1024}, NDColorModeRGB1};
    appendTestCase(&overlayTestCaseStrs, &test6); 
    // Test an case with very large width
    overlayTempCaseStr test7 = {0, 500, 500, 50, 50, 5000, 5000, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test7); 
    // Test an case with zero width
    overlayTempCaseStr test8 = {0, 500, 500, 50, 50, 0, 0, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test8);
    // Test a "normal" case using address 1
    overlayTempCaseStr test9 = {1, 500, 500, 50, 50, 1, 1, 0, 255, 0, NDOverlayCross, NDOverlaySet, 2, {1024, 1024}, NDColorModeMono};
    appendTestCase(&overlayTestCaseStrs, &test9);
  }
Exemple #11
0
      void Add( const boost::shared_ptr<T>& prt )
      {
	if ( prt_.empty() )
	  prt->SetFocus();
	prt_.push_back(prt);
      }
Exemple #12
0
void print_fun(boost::shared_ptr<int> value)
{cout<<"fun : value is = "<<*value<<", use_count = "<<value.use_count()<<endl;}
Exemple #13
0
		void print()
		{cout<<"class : value is = "<<*p_<<", use_count = "<<p_.use_count()<<endl;}
Exemple #14
0
void OgrWriter::writePartial(const boost::shared_ptr<const hoot::Relation>& newRelation)
{
  // Make sure all the elements in the relation are in the cache
  const std::vector<RelationData::Entry>& relationEntries = newRelation->getMembers();

  std::vector<RelationData::Entry>::const_iterator relationElementIter;
  long nodeCount = 0;
  long wayCount = 0;
  long relationCount = 0;

  for ( relationElementIter = relationEntries.begin();
      relationElementIter != relationEntries.end();
      relationElementIter++ )
  {
    switch ( relationElementIter->getElementId().getType().getEnum() )
    {
      case ElementType::Node:
        nodeCount++;
        if ( nodeCount > _currElementCacheCapacity )
        {
          LOG_FATAL("Relation ID " << newRelation->getId() <<
            " contains more nodes than can fit in the cache (" << _currElementCacheCapacity<<
            ")");
          throw HootException("Relation with too many nodes");
        }
        break;
      case ElementType::Way:
        wayCount++;
        if ( wayCount > _currElementCacheCapacity)
        {
          LOG_FATAL("Relation ID " << newRelation->getId() <<
            " contains more ways than can fit in the cache (" << _currElementCacheCapacity <<
            ")");
          throw HootException("Relation with too many ways");
        }

        break;
      case ElementType::Relation:
        relationCount++;
        if ( relationCount > _currElementCacheCapacity)
        {
          LOG_FATAL("Relation ID " << newRelation->getId() <<
            " contains more relations than can fit in the cache (" << _currElementCacheCapacity <<
            ")");
          throw HootException("Relation with too many relations");
        }

        break;
      default:
        throw HootException("Relation containus unknown type");
        break;
    }

    if ( _elementCache->containsElement(relationElementIter->getElementId()) == false )
    {
      throw HootException("Relation element did not exist in cache");
    }
  }

  // Add to the cache
  ConstElementPtr constRelation(newRelation);
  _elementCache->addElement(constRelation);

  ElementProviderPtr cacheProvider(_elementCache);
    _writePartial(cacheProvider, newRelation);
}
cql::cql_message_query_impl_t::cql_message_query_impl_t(const boost::shared_ptr<cql_query_t>& query) 
	: _buffer(new std::vector<cql_byte_t>())
	, _consistency(query->consistency())
	, _query(query->query())
    , _is_traced(query->is_traced())
{}
Acceptor::Acceptor(boost::shared_ptr< Hive > hive)
	: m_hive(hive), m_acceptor(hive->GetService()), m_io_strand(hive->GetService()), m_timer(hive->GetService()), m_timer_interval(1000), m_error_state(0)
{
}
Exemple #17
0
void PeekProcessor::peek(boost::shared_ptr<TProtocol> in,
                         TType ftype,
                         int16_t fid) {
  (void) fid;
  in->skip(ftype);
}
void Acceptor::DispatchAccept(boost::shared_ptr< Connection > connection)
{
	m_acceptor.async_accept(connection->GetSocket(), connection->GetStrand().wrap(boost::bind(&Acceptor::HandleAccept, shared_from_this(), _1, connection)));
}
 ConsistencyTests()
   : ccm(new CCM::Bridge("config.txt"))
   , ip_prefix(ccm->get_ip_prefix()) {}
void compare_scans(boost::shared_ptr<vector<line> > firstScan,
                   boost::shared_ptr<vector<line> > secondScan,
                   int rotation_prior_mean,
                   int& rotOut,
                   Vector2f translation_prior_mean,
                   Vector2f & translationOut)

{

    int num_theta_entries = (2 * M_PI -
                             delta_theta*angle_increment)/(delta_theta*M_PI/180) + 1;
    vector<float> theta = vector<float>(num_theta_entries);
    for(double i = -M_PI, counter = 0; counter < num_theta_entries;
            i+= delta_theta*M_PI/180, counter++) {
        theta[counter] = i;

    }





    //Initial rotation estimation. This will vary depending on the
    //lines being compared.

    //    Vector2f translation_prior_mean = previous_translation;
    float translation_prior_sd = 0.5;

    //    float rotation_prior_mean = previous_est_rot;
    float rotation_prior_sd = 15;
    float rotation_posterior_sd = 4;

    vector<float> rotation_prior (360,0);
    vector<float> rotation_score (360,0);

    float dist;
    float sum1;
    for (int i=0; i<360; i++)
    {
        dist = min(abs(i - rotation_prior_mean),abs(i-360-rotation_prior_mean));
        rotation_prior[i] = exp(-pow(dist,2)/pow(rotation_prior_sd,2));
        sum1 += rotation_prior[i];
    }
    for (int i=0; i<360; i++)
    {
        rotation_prior[i] /= sum1;
    }



    //for each pair of lines between the two scans, estimate the
    // probability that they are from the same object. The more likely they
    //   are to be by an unmoved object, the more likely that the rotation of
    //   the was equal to the theta difference between the lines.

    for (int i=0; i < firstScan->size(); i++)
    {
        for (int j=0; j < secondScan->size(); j++)
        {
            float dist;
            float theta_diff = (delta_theta*(firstScan->at(i).theta_index -
                                             secondScan->at(j).theta_index) );
            if (theta_diff < 0) {
                theta_diff += 360;
            }

            //if the lines are clearly not from the same source because the
            // translation has moved too much for one frame, then do not use their
            // 							    relative angles to estimate the rotation.
            float expected_rho_change;
            expected_rho_change =
                -(translation_prior_mean[0]*cos(theta[firstScan->at(i).theta_index]) +
                  translation_prior_mean(1)*sin(theta[firstScan->at(i).theta_index]) );
            if(abs(firstScan->at(i).est_rho + expected_rho_change -
                    secondScan->at(j).est_rho) > rho_sanity_tolerance)
            {
                continue;
            }

            for (int k=0; k<360; k++)
            {
                dist = min(abs(k-theta_diff), abs(k-theta_diff-360));
                rotation_score[k] = rotation_score[k] +
                                    exp(-pow(dist,2)/pow(rotation_posterior_sd,2));
            }
        }
    }

    vector<float> rotation_prob(360,0);

    for (int i=0; i<rotation_prob.size(); i++) {
        rotation_prob[i] = rotation_prior[i]* rotation_score[i];
        //rotation_prob[i] =  rotation_score[i];
    }

    float max1 = 0;


    for (int i=0; i<rotation_prob.size(); i++) {
        if (rotation_prob[i]> max1) {
            max1 = rotation_prob[i];
            rotOut = i;
        }
    }

    cout << "est rot: " << rotOut << endl;

    vector<vector<int> > matched_lines;
    for (int i=0; i < firstScan->size(); i++)
    {
        for (int j=0; j < secondScan->size(); j++)
        {
            if (min(abs(delta_theta*(firstScan->at(i).theta_index -
                                     secondScan->at(j).theta_index) - rotOut),
                    abs(delta_theta*(firstScan->at(i).theta_index -
                                     secondScan->at(j).theta_index) - rotOut + 360))< 10)
            {

                float expected_rho_change;
                expected_rho_change =
                    -(translation_prior_mean[0]*cos(theta[firstScan->at(i).theta_index]) +
                      translation_prior_mean(1)*sin(theta[firstScan->at(i).theta_index]) );
                if(abs(firstScan->at(i).est_rho + expected_rho_change -
                        secondScan->at(j).est_rho) < rho_sanity_tolerance)
                {
                    vector<int> temp_vector(2,0);
                    temp_vector[0] = i;
                    temp_vector[1] = j;
                    matched_lines.push_back(temp_vector);
                    break;
                }
            }
        }
    }

    if (matched_lines.size() > 0)
    {


        //compute the change in rho for each pair of matched lines
        MatrixXf rho_change(matched_lines.size(),1);
        for (int i=0; i<matched_lines.size(); i++)
        {
            rho_change(i,0) = firstScan->at(matched_lines[i][0]).est_rho -
                              secondScan->at(matched_lines[i][1]).est_rho;
        }


        MatrixXf A(matched_lines.size(),2);
        for (int i=0; i<matched_lines.size(); i++)
        {

            A(i,0) = cos(theta[firstScan->at(matched_lines[i][0]).theta_index]);
            A(i,1) = sin(theta[firstScan->at(matched_lines[i][0]).theta_index]);


            //          A(i,0) = cos(theta[secondScan->at(matched_lines[i][1]).theta_index]);
            //          A(i,1) = sin(theta[secondScan->at(matched_lines[i][1]).theta_index]);

            cout << A(i,0) << "  " << A(i,1) << endl;
        }




        float condition_threshold = 0.2;

        Matrix2f C = A.transpose()*A;
        SelfAdjointEigenSolver<Matrix2f> eigensolver(C);
        Vector2f Eigenvalues = eigensolver.eigenvalues();
        Matrix2f Eigenvectors = eigensolver.eigenvectors();
        MatrixXd observed_projection(2,2);


        if (Eigenvalues(0) == 0) {
            Eigenvalues(0) = 0.000001;
        }
        if (Eigenvalues(1) == 0) {
            Eigenvalues(1) = 0.000001;
        }
        cout << "condition number: " << Eigenvalues(0)/Eigenvalues(1) << endl;
        if (Eigenvalues(0)/Eigenvalues(1) < condition_threshold ||
                Eigenvalues(0)/Eigenvalues(1) > 1/condition_threshold)
        {
            int principal_eigenvalue = 0;
            if (Eigenvalues(1) > Eigenvalues(0))
                principal_eigenvalue = 1;

            MatrixXd unobserved_direction(2,1);
            unobserved_direction(0) = Eigenvectors(0,1-principal_eigenvalue);
            unobserved_direction(1) = Eigenvectors(1,1-principal_eigenvalue);

            MatrixXd observed_direction(2,1);// = Eigenvectors(0,1,2,1);
            observed_direction(0) = Eigenvectors(0,principal_eigenvalue);
            observed_direction(1) = Eigenvectors(1,principal_eigenvalue);

            //cout << "principal_eigenvalue: " << principal_eigenvalue << endl;
            //cout << "Eigenvectors: " << endl;
            //cout << Eigenvectors;

            observed_projection =
                observed_direction*(observed_direction.transpose()*observed_direction).inverse()*observed_direction.transpose();

        }
        else {
            observed_projection << 1, 0,
                                0, 1;
        }

        Vector2f currentTranslation = A.jacobiSvd(ComputeThinU |
                                      ComputeThinV).solve(rho_change);


        Matrix2f observed_projection2f;
        observed_projection2f(0,0) = observed_projection(0,0);
        observed_projection2f(0,1) = observed_projection(0,1);
        observed_projection2f(1,0) = observed_projection(1,0);
        observed_projection2f(1,1) = observed_projection(1,1);

        //translationOut = observed_projection2f * currentTranslation +
        // (sourceTranslation - observed_projection2f*sourceTranslation);
        // translationOut = observed_projection2f * currentTranslation +
        // 	(previousTranslation - observed_projection2f*previousTranslation);
        translationOut = currentTranslation;


        if (rotOut > 180) {
            rotOut -= 360;

        }
        cout << endl << "Estimated Rotation:  " << rotOut;
        //cout<< "   Estimated Translation:  " << translationOut(0) << "   "
        // << translationOut(1) << "   MatchedLines: " << matched_lines.size() <<
        //endl;
        cout<< "   Estimated Translation:  " << base_translation(0) +
            translationOut(0) << "   " << base_translation(1) + translationOut(1)
            << "   MatchedLines: " << matched_lines.size() << endl;

    }
}
void
SessionManager::addDataprocessor( boost::shared_ptr<Dataprocessor>& proc )
{
    sessions_.push_back( Session( proc ) );
    emit signalSessionAdded( proc.get() );
}
 ~Compare() {
     scan1.reset();
     scan2.reset();
 }
inline std::vector<std::string> const &ProblemData::getClassLabels() const
{
    return *ptrClassLabels.get();
}
    void lines_callback(const art_lrf::Lines::ConstPtr& msg_lines) {

        if ((msg_lines->theta_index.size() != 0) &&
                (msg_lines->est_rho.size() != 0) && (msg_lines->endpoints.size() !=
                        0)) {


            scan2.reset(new vector<line> ());
            line temp;
            geometry_msgs::Point32 temp2;
            geometry_msgs::Point32 temp_theta_index;
            vector<int> temp3;

            for (int i = 0; i < msg_lines->theta_index.size(); i++) {
                temp.theta_index = msg_lines->theta_index[i];
                temp.est_rho = msg_lines->est_rho[i];

                for (int j = 0; j < msg_lines->endpoints[i].points.size(); j++) {
                    temp2 = msg_lines->endpoints[i].points[j];
                    temp3.push_back(temp2.x);
                    temp3.push_back(temp2.y);
                    temp.endpoints.push_back(temp3);
                }


                scan2->push_back(temp);
                temp.endpoints.clear();
            }

            if (firstRun == true) {
                firstRun = false;
                scan1 = scan2;
                previous_translation << 0,0;
            }
            else
            {

                compare_scans(scan1, scan2, previous_est_rot, est_rot,
                              previous_translation, est_translation );

                if( pow(pow(est_translation[0],2) + pow(est_translation[1],2),0.5) >
                        WAYPOINT_THRESHOLD)
                {
                    measurement new_base;
                    for(int i=0; i<scan1->size(); i++)
                    {
                        new_base.lines.push_back(scan1->at(i));
                    }
                    new_base.x = base_translation(0) + est_translation(0);
                    new_base.y = base_translation(1) + est_translation(1);
                    new_base.yaw = base_rot;

                    // float min_dist = 2*REMATCH_CANDIDATE_THRESH; //start with
                    //	      arbitrarily high value
                    // int min_index = 0;
                    // for( int i=0; i<base_scans.size(); i++)
                    // {
                    //  float current_dist = base_scans[i].dist_from(new_base);
                    //  if( current_dist < min_dist )
                    //  {
                    //      min_index = i;
                    //      min_dist = current_dist;
                    //  }
                    // }
                    // if(min_dist < REMATCH_CANDIDATE_THRESH)
                    // {
                    //  boost::shared_ptr<vector<line> > temp_boost_scan(new vector<line>);
                    //  for(int i=0; i<base_scans[min_index].lines.size(); i++)
                    //  {
                    //      temp_boost_scan->push_back(base_scans[min_index].lines[i]);
                    //  }

                    //  int expected_rot_difference = new_base.yaw -
                    //base_scans[min_index].yaw;
                    //  int actual_rot_difference;
                    //  Vector2f expected_translation_difference;
                    //  expected_translation_difference << new_base.x -
                    //base_scans[min_index].x, new_base.y - base_scans[min_index].y;
                    //  Vector2f actual_translation_difference;

                    //  compare_scans(temp_boost_scan, scan2,
                    //                new_base.yaw - base_scans[min_index].yaw,
                    //actual_rot_difference,
                    //                expected_translation_difference,
                    //actual_translation_difference );
                    //  float translation_error =
                    // pow(pow(expected_translation_difference(0) -
                    // 	  actual_translation_difference(0), 2) +
                    //     pow(expected_translation_difference(1) -
                    // 	  actual_translation_difference(1),2), 0.5);
                    //  if( (translation_error < REMATCH_VALIDATION_THRESH) &&
                    //(abs(expected_rot_difference - actual_rot_difference) < 7))
                    //  {
                    //      //overwrite the new_base values with the difference
                    //between the waypoint and the current scan
                    //      new_base.x = base_scans[min_index].x +
                    //actual_translation_difference[0];
                    //      new_base.y = base_scans[min_index].y +
                    //actual_translation_difference[1];
                    //;	//      new_base.yaw = base_scans[min_index].yaw + actual_rot_difference;
                    //  }
                    // }



                    scan1 = scan2;
                    base_translation << new_base.x, new_base.y;
                    base_rot = new_base.yaw;
                    base_scans.push_back(new_base);
                    est_translation << 0, 0;
                    est_rot = 0;
                }

                previous_translation = est_translation;
                previous_est_rot = est_rot;


            }



            geometry_msgs::Point32 pos_msg;
            pos_msg.x = est_translation(0);
            pos_msg.y = est_translation(1);
            pos_msg.z = est_rot;

            pub_pos.publish(pos_msg);

            pos_msg_old.x = pos_msg.x;
            pos_msg_old.y = pos_msg.y;
            pos_msg_old.z = pos_msg.z;
        }



        else {
            cout << endl << "MISSED SCAN" << endl;
            pub_pos.publish(pos_msg_old);
        }

    }
inline std::vector<arma::vec> const &ProblemData::getSamples() const
{
    return *ptrSamples.get();
}
Exemple #26
0
 // this makes sure we can store our connections in a set
 inline bool operator<(boost::shared_ptr<receiver> const& lhs,
     boost::shared_ptr<receiver> const& rhs)
 {
     return lhs.get() < rhs.get();
 }
boost::shared_ptr<HierarchyDataOpsReal<double> >
HierarchyDataOpsManager::getOperationsDouble(
   const boost::shared_ptr<hier::Variable>& variable,
   const boost::shared_ptr<hier::PatchHierarchy>& hierarchy,
   bool get_unique)
{
   TBOX_ASSERT(variable);
   TBOX_ASSERT(hierarchy);
   TBOX_ASSERT_OBJDIM_EQUALITY2(*variable, *hierarchy);

   const boost::shared_ptr<pdat::CellVariable<double> > cellvar(
      boost::dynamic_pointer_cast<pdat::CellVariable<double>,
                                  hier::Variable>(variable));
   const boost::shared_ptr<pdat::FaceVariable<double> > facevar(
      boost::dynamic_pointer_cast<pdat::FaceVariable<double>,
                                  hier::Variable>(variable));
   const boost::shared_ptr<pdat::NodeVariable<double> > nodevar(
      boost::dynamic_pointer_cast<pdat::NodeVariable<double>,
                                  hier::Variable>(variable));
   const boost::shared_ptr<pdat::SideVariable<double> > sidevar(
      boost::dynamic_pointer_cast<pdat::SideVariable<double>,
                                  hier::Variable>(variable));
   const boost::shared_ptr<pdat::EdgeVariable<double> > edgevar(
      boost::dynamic_pointer_cast<pdat::EdgeVariable<double>,
                                  hier::Variable>(variable));

   boost::shared_ptr<HierarchyDataOpsReal<double> > ops;

   if (cellvar) {

      if (get_unique) {
         ops.reset(new HierarchyCellDataOpsReal<double>(hierarchy));
      } else {
         const int n = static_cast<int>(d_cell_ops_double.size());
         for (int i = 0; i < n && !ops; ++i) {
            if (hierarchy !=
                d_cell_ops_double[i]->getPatchHierarchy()) continue;
            // A compatible operator has been found at i.
            ops = d_cell_ops_double[i];
         }
         if (!ops) {
            // No compatible operator has been found.
            ops.reset(new HierarchyCellDataOpsReal<double>(hierarchy));
            d_cell_ops_double.resize(n + 1);
            d_cell_ops_double[n] = ops;
         }
      }

   } else if (facevar) {

      if (get_unique) {
         ops.reset(new HierarchyFaceDataOpsReal<double>(hierarchy));
      } else {
         const int n = static_cast<int>(d_face_ops_double.size());
         for (int i = 0; i < n && !ops; ++i) {
            if (hierarchy !=
                d_face_ops_double[i]->getPatchHierarchy()) continue;
            // A compatible operator has been found at i.
            ops = d_face_ops_double[i];
         }
         if (!ops) {
            // No compatible operator has been found.
            ops.reset(new HierarchyFaceDataOpsReal<double>(hierarchy));
            d_face_ops_double.resize(n + 1);
            d_face_ops_double[n] = ops;
         }
      }

   } else if (nodevar) {

      if (get_unique) {
         ops.reset(new HierarchyNodeDataOpsReal<double>(hierarchy));
      } else {
         const int n = static_cast<int>(d_node_ops_double.size());
         for (int i = 0; i < n && !ops; ++i) {
            if (hierarchy !=
                d_node_ops_double[i]->getPatchHierarchy()) continue;
            // A compatible operator has been found at i.
            ops = d_node_ops_double[i];
         }
         if (!ops) {
            // No compatible operator has been found.
            ops.reset(new HierarchyNodeDataOpsReal<double>(hierarchy));
            d_node_ops_double.resize(n + 1);
            d_node_ops_double[n] = ops;
         }
      }

   } else if (sidevar) {

      if (get_unique) {
         ops.reset(new HierarchySideDataOpsReal<double>(hierarchy));
      } else {
         const int n = static_cast<int>(d_side_ops_double.size());
         for (int i = 0; i < n && !ops; ++i) {
            if (hierarchy !=
                d_side_ops_double[i]->getPatchHierarchy()) continue;
            // A compatible operator has been found at i.
            ops = d_side_ops_double[i];
         }
         if (!ops) {
            // No compatible operator has been found.
            ops.reset(new HierarchySideDataOpsReal<double>(hierarchy));
            d_side_ops_double.resize(n + 1);
            d_side_ops_double[n] = ops;
         }
      }

   } else if (edgevar) {

      if (get_unique) {
         ops.reset(new HierarchyEdgeDataOpsReal<double>(hierarchy));
      } else {
         const int n = static_cast<int>(d_edge_ops_double.size());
         for (int i = 0; i < n && !ops; ++i) {
            if (hierarchy !=
                d_edge_ops_double[i]->getPatchHierarchy()) continue;
            // A compatible operator has been found at i.
            ops = d_edge_ops_double[i];
         }
         if (!ops) {
            // No compatible operator has been found.
            ops.reset(new HierarchyEdgeDataOpsReal<double>(hierarchy));
            d_edge_ops_double.resize(n + 1);
            d_edge_ops_double[n] = ops;
         }
      }

   }

   if (!ops) {
      TBOX_ERROR("HierarchyDataOpsManager internal error...\n"
         << "Operations for variable " << variable->getName()
         << " not defined.");
   }

   return ops;
}
void YOGLoginScreen::handleYOGClientEvent(boost::shared_ptr<YOGClientEvent> event)
{
	//std::cout<<"YOGLoginScreen: recieved event "<<event->format()<<std::endl;
	Uint8 type = event->getEventType();
	if(type == YEConnected)
	{
		attemptLogin();
	}
	else if(type == YEConnectionLost)
	{ 
		//shared_ptr<YOGConnectionLostEvent> info = static_pointer_cast<YOGConnectionLostEvent>(event);
		animation->visible=false;
		statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_LOST]"));
	}
	else if(type == YELoginAccepted)
	{
		//shared_ptr<YOGLoginAcceptedEvent> info = static_pointer_cast<YOGLoginAcceptedEvent>(event);
		animation->visible=false;
		runLobby();
	}
	else if(type == YELoginRefused)
	{
		shared_ptr<YOGLoginRefusedEvent> info = static_pointer_cast<YOGLoginRefusedEvent>(event);
		animation->visible=false;
		YOGLoginState reason = info->getReason();
		if(reason == YOGPasswordIncorrect)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_BAD_PASSWORD]"));
		}
		else if(reason == YOGUsernameAlreadyUsed)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_ALREADY_PASSWORD]"));
		}
		else if(reason == YOGUserNotRegistered)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_BAD_PASSWORD_NON_ZERO]"));
		}
		else if(reason == YOGClientVersionTooOld)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_PROTOCOL_TOO_OLD]"));
		}
		else if(reason == YOGAlreadyAuthenticated)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_USERNAME_ALLREADY_USED]"));
		}
		else if(reason == YOGUsernameBanned)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_USERNAME_BANNED]"));
		}
		else if(reason == YOGIPAddressBanned)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_IP_TEMPORARILY_BANNED]"));
		}
		else if(reason == YOGNameInvalidSpecialCharacters)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_USERNAME_INVALID_SPECIAL_CHARACTERS]"));
		}
		else if(reason == YOGLoginUnknown)
		{
			statusText->setText(Toolkit::getStringTable()->getString("[YESTS_CONNECTION_REFUSED_UNEXPLAINED]"));
		}
		client->disconnect();
	}
}
  void _modifyTests(SplitLongLinearWaysVisitor& splitVisitor)
  {

    int wayID = 0;
    int startNode = 234;
    int numNodes;
    int numWays;

    // N + 1 nodes: 2 ways
    _map.reset(new OsmMap());
    numNodes = splitVisitor.getMaxNumberOfNodes() + 1;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    WayMap ways = _map->getWays();
    CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // N + 2 nodes: 2 ways
    _map.reset(new OsmMap());
    numNodes = splitVisitor.getMaxNumberOfNodes() + 2;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    ways = _map->getWays();
    CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;

    // (2N) - 2 nodes: 2 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) - 2;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    ways = _map->getWays();
    CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (2N) - 1 nodes: 2 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) - 1;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    ways = _map->getWays();
    CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (2N) nodes: 2 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 2);
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    ways = _map->getWays();
    CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (2N) + 1 nodes: 3 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) + 1;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    ways = _map->getWays();
    CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (2N) + 2 nodes: 3 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 2) + 2;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    ways = _map->getWays();
    CPPUNIT_ASSERT_EQUAL( static_cast<size_t>(numWays), ways.size() );
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;



    // Picking 541N as 541 is large enough to be a good stress test, plus it's a prime number
    //    and primes are good at exposing problems

    // (541N) - 2 nodes: 541 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) - 2;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (541N) - 1 nodes: 541 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) - 1;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (541N) nodes: 541 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 541);
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (541N) + 1 nodes: 542 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) + 1;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;


    // (541N) + 2 nodes: 542 ways
    _map.reset(new OsmMap());
    numNodes = (splitVisitor.getMaxNumberOfNodes() * 541) + 2;
    numWays = _calcNumWays(numNodes, splitVisitor);
    _createWay(++wayID, startNode, startNode + numNodes - 1);
    _map->visitRw(splitVisitor);

    // Sanity checks on split
    _sanityCheckSplit(splitVisitor, startNode, numNodes, numWays);
    startNode += numNodes;
  }
Exemple #30
0
void robox_parameters_load(robox_parameters & params,
			  boost::shared_ptr<sfl::OptionDictionary> opt)
{
  expo_parameters_load(params, opt);

  string_to(opt->GetOption("front_nscans"), params.front_nscans);
  string_to(opt->GetOption("front_mount_x"), params.front_mount_x);
  string_to(opt->GetOption("front_mount_y"), params.front_mount_y);
  string_to(opt->GetOption("front_mount_theta"), params.front_mount_theta);
  string_to(opt->GetOption("front_rhomax"), params.front_rhomax);
  string_to(opt->GetOption("front_phi0"), params.front_phi0);
  string_to(opt->GetOption("front_phirange"), params.front_phirange);
  
  string_to(opt->GetOption("rear_nscans"), params.rear_nscans);
  string_to(opt->GetOption("rear_mount_x"), params.rear_mount_x);
  string_to(opt->GetOption("rear_mount_y"), params.rear_mount_y);
  string_to(opt->GetOption("rear_mount_theta"), params.rear_mount_theta);
  string_to(opt->GetOption("rear_rhomax"), params.rear_rhomax);
  string_to(opt->GetOption("rear_phi0"), params.rear_phi0);
  string_to(opt->GetOption("rear_phirange"), params.rear_phirange);
}