C++ Estimator类代码示例

bool estimation (string* dataName, string* outputName) {
    bool result = true;
    cout << "[Log] Allocating Estimator object.\n";
    Estimator* estimator = new Estimator(dataName, outputName);
    cout << "[Log] Starting estimation. \n";
    if (!estimator->start()) {
        result = false;
    }

    delete estimator;

    return result;
}

Controller::Controller(	ControlLaw& _controlLaw,
						Estimator& _estimator,
						ReferenceTrajectory& _referenceTrajectory
						) : SimulationBlock( BN_CONTROLLER )
{
	setupOptions( );
	setupLogging( );

	if ( _controlLaw.isDefined( ) == BT_TRUE )
	{
		controlLaw = &_controlLaw;
		setStatus( BS_NOT_INITIALIZED );
	}
	else
		controlLaw = 0;

	if ( _estimator.isDefined( ) == BT_TRUE )
		estimator = &_estimator;
	else
		estimator = 0;

	if ( _referenceTrajectory.isDefined( ) == BT_TRUE )
		referenceTrajectory = &_referenceTrajectory;
	else
		referenceTrajectory = 0;
	
	setStatus( BS_NOT_INITIALIZED );
}

Vector getSimulatedListener(EventLog *event)
{
#if 0
	return event->location;
#else
	unsigned long timestamp = event->timestamp;
	for (size_t i = 0; i < event->measurements.size(); i++)
	{
		estimator.measure(timestamp, event->measurements[i].userBid, event->measurements[i].distance);
	}
	EstimatorResult result = estimator.solve(timestamp);
	
//	printf("%f\n", event->location.getDistance(result.location));

	return result.location;
#endif
}

std::vector<typename Estimator::Quantity>
estimateQuantity( Estimator & estimator, 
      ConstIterator it, ConstIterator it_end )
{
  std::vector<typename Estimator::Quantity> values;
  for ( ; it != it_end; ++it )
    {
      values.push_back( estimator.eval( it ) );
    }
  return values;
}

void computeEstimation
( const po::variables_map& vm,     //< command-line parameters
  const KSpace& K,                 //< cellular grid space
  const ImplicitShape& shape,      //< implicit shape "ground truth"
  const Surface& surface,          //< digital surface approximating shape
  Estimator& estimator )           //< an initialized estimator
{
  typedef typename Surface::ConstIterator ConstIterator;
  typedef typename Surface::Surfel Surfel;
  typedef typename Estimator::Quantity Quantity;
  typedef double Scalar;
  typedef DepthFirstVisitor< Surface > Visitor;
  typedef GraphVisitorRange< Visitor > VisitorRange;
  typedef typename VisitorRange::ConstIterator VisitorConstIterator;
  
  std::string fname = vm[ "output" ].as<std::string>();
  string nameEstimator = vm[ "estimator" ].as<string>();

  trace.beginBlock( "Computing " + nameEstimator + " estimations." );
  CountedPtr<VisitorRange> range( new VisitorRange( new Visitor( surface, *(surface.begin()) )) );
  std::vector<Quantity> n_estimations;
  estimator.eval( range->begin(), range->end(), std::back_inserter( n_estimations ) );
  trace.info() << "- nb estimations  = " << n_estimations.size() << std::endl;
  trace.endBlock();

  trace.beginBlock( "Computing areas." );
  range = CountedPtr<VisitorRange>( new VisitorRange( new Visitor( surface, *(surface.begin()) )) );
  double area_est   = 0.0; // normal integration with absolute value.
  unsigned int i = 0;
  for ( typename VisitorRange::ConstIterator it = range->begin(), itE = range->end(); 
        it != itE; ++it, ++i )
    {
      Surfel s = *it;
      Dimension k = K.sOrthDir( s );
      area_est  += abs( n_estimations[ i ][ k ] );
    }  
  double h = vm["gridstep"].as<double>();
  trace.info() << setprecision(10) << "- Area_est     " << ( area_est * h * h )   << std::endl;
  std::ostringstream area_sstr;
  area_sstr << fname << "-" << nameEstimator << "-area-" << h << ".txt"; 
  std::ofstream area_output( area_sstr.str().c_str() );
  area_output << "# Area estimation by digital surface integration." << std::endl;
  area_output << "# X: " << nameEstimator << std::endl;
  area_output << "# h Area[X] nb_surf" << std::endl;
  area_output << setprecision(10) << h
              << " " << ( area_est * h * h )
              << " " << i << std::endl;
  area_output.close();
  trace.endBlock();
}

returnValue Controller::setEstimator(	Estimator& _estimator
										)
{
	if ( _estimator.isDefined( ) == BT_TRUE )
	{
		if ( estimator == 0 )
			estimator = &_estimator;
		else
			*estimator = _estimator;

		if ( getStatus( ) > BS_NOT_INITIALIZED )
			setStatus( BS_NOT_INITIALIZED );
	}
	
	return SUCCESSFUL_RETURN;
}

void exportNOFFSurface( const DigitalSurface& surface,
                        const Estimator& estimator,
                        std::ostream& output )
{
  typedef typename DigitalSurface::KSpace KSpace;
  typedef typename DigitalSurface::ConstIterator ConstIterator;
  typedef typename DigitalSurface::Surfel Surfel;
  typedef typename KSpace::SCell SCell;
  typedef typename Estimator::Quantity Quantity;
  const KSpace& ks = surface.container().space();
  std::map<Surfel,Quantity> normals;
  for ( ConstIterator it = surface.begin(), itE = surface.end(); it != itE; ++it )
    {
      Quantity n_est = estimator.eval( it );
      normals[ *it ] = n_est;
    }
  CanonicSCellEmbedder<KSpace> surfelEmbedder( ks );
  typedef SCellEmbedderWithNormal< CanonicSCellEmbedder<KSpace> > Embedder;
  Embedder embedder( surfelEmbedder, normals );
  surface.exportAs3DNOFF( output, embedder );
}

int main(int argc, char** argv)
{
	char configFilename[256];
	
	if (argc > 1)
		strncpy(configFilename, argv[1], 255);
	else
		strcpy(configFilename, "conf.txt");

	SimulatorArgument simArgs;
	simArgs.load(configFilename);

	// real plane should be saved as plane.auto.conf, but for convenice, save plane.detected.conf
	// this will be rewrite after detecting plane process
	simArgs.planes.save("plane.detected.conf");
	simArgs.beacons.save("beacon.auto.conf");

	estArgs.load(configFilename);
	estArgs.estimatorMode = EST::TRADITIONAL;
	estArgs.optimization = 0;
	estimator.setEstimator(&estArgs);

	

	PlaneList realPlanes;

	PlaneList detectedPlanes;

	realPlanes.makeCube(simArgs.width, simArgs.length, simArgs.height);
	// there is 6 planes in realPlanes.
	// index 0~3 are side wall, 4 and 5 are ceiling and floor
	

	printf("Generating direct signal events...");
	fflush(stdout);
	// events for directly receiving signals
	EventGenerator directEventGenerator;
	directEventGenerator.generateEventForPlaneDetection(
			&simArgs, listenerInterval, minMargin, listenerZ, Vector(0, 0, 1));
	printf("done\n");

	Vector bestLocation[4];
	Vector bestLocationSimulated[4];
	double error[4];

	
	for (int i = 0; i < 4; i++)
	{
		EventGenerator reflectedEventGenerator;
		// normal vector of each plane faces outside of cube
		Vector vFacing = realPlanes.at(i)->vNormal;

		printf("\nGenerating events for Plane %d...", i);
		fflush(stdout);
		reflectedEventGenerator.generateEventForPlaneDetection(
				&simArgs, listenerInterval, minMargin, listenerZ, vFacing);
		printf("done\n");

		double gap = 100;

		Vector fixedPoint;
		int fixedPointIdx;
		EventLog *eventD;
		EventLog *eventR;
		
		for (gap = 100; gap > 0; gap -= 5)
		{

			fixedPoint = getFixedPoint(realPlanes.at(i), gap);
			fixedPointIdx = getFixedPointIndex(fixedPoint, &directEventGenerator.events);
			eventD = &directEventGenerator.events.events[fixedPointIdx];
			eventR = &reflectedEventGenerator.events.events[fixedPointIdx];

			if (!checkReflection(eventD)) continue;
			if (eventR->measurements.size() >= 5) break;
		};
		if (gap <= 0)
		{
			fprintf(stderr, "can not find valid point for plane %d\n", i);
			exit(1);
		}

		printf("fixed point for plane %d  (gap : %d) = ", i, (int)gap);
		fixedPoint.println();

		Plane detectedPlane = detectPlane(
				&simArgs, 
				realPlanes.at(i), 
				i/*pid*/, 				
				eventD,
				eventR,
				&bestLocation[i],
				&bestLocationSimulated[i],
				&error[i]);
		detectedPlane.setBoundary(true);
		detectedPlanes.addPlane(detectedPlane);
		printf("min error : %f\n", error[i]);
	}

	for (int i = 0; i < 5; i++)
//.........这里部分代码省略.........

void chooseEstimator
( const po::variables_map& vm,     //< command-line parameters
  const KSpace& K,                 //< cellular grid space
  const ImplicitShape& shape, //< implicit shape "ground truth"
  const Surface& surface,     //< digital surface approximating shape
  const KernelFunction& chi,  //< the kernel function
  const PointPredicate& ptPred )   //< analysed implicit digital shape as a PointPredicate
{
  using namespace DGtal::functors;
  string nameEstimator = vm[ "estimator" ].as<string>();
  double h = vm["gridstep"].as<double>();
  typedef ShapeGeometricFunctors::ShapeNormalVectorFunctor<ImplicitShape> NormalFunctor;
  typedef TrueDigitalSurfaceLocalEstimator<KSpace, ImplicitShape, NormalFunctor> TrueEstimator;
  TrueEstimator true_estimator;
  true_estimator.attach( shape );
  true_estimator.setParams( K, NormalFunctor(), 20, 0.1, 0.01 );
  true_estimator.init( h, surface.begin(), surface.end() );
  if ( nameEstimator == "True" )
    {
      trace.beginBlock( "Chosen estimator is: True." );
      typedef TrueDigitalSurfaceLocalEstimator<KSpace, ImplicitShape, NormalFunctor> Estimator;
      int maxIter     = vm["maxiter"].as<int>();
      double accuracy = vm["accuracy"].as<double>();
      double gamma    = vm["gamma"].as<double>();
      Estimator estimator;
      estimator.attach( shape );
      estimator.setParams( K, NormalFunctor(), maxIter, accuracy, gamma );
      estimator.init( h, surface.begin(), surface.end() );
      trace.endBlock();
      computeEstimation( vm, K, shape, surface, estimator );
    }
  else if ( nameEstimator == "VCM" )
    {
      trace.beginBlock( "Chosen estimator is: VCM." );
      typedef typename KSpace::Space Space;
      typedef typename Surface::DigitalSurfaceContainer SurfaceContainer;
      typedef ExactPredicateLpSeparableMetric<Space,2> Metric;
      typedef VoronoiCovarianceMeasureOnDigitalSurface<SurfaceContainer,Metric,
                                                       KernelFunction> VCMOnSurface;
      typedef VCMNormalVectorFunctor<VCMOnSurface> NormalFunctor;
      typedef VCMDigitalSurfaceLocalEstimator<SurfaceContainer,Metric,
                                              KernelFunction, NormalFunctor> VCMNormalEstimator;
      int embedding = vm["embedding"].as<int>();
      Surfel2PointEmbedding embType = embedding == 0 ? Pointels :
                                      embedding == 1 ? InnerSpel : OuterSpel;     
      double R = vm["R-radius"].as<double>();
      double r = vm["r-radius"].as<double>();
      double t = vm["trivial-radius"].as<double>();
      double alpha = vm["alpha"].as<double>();
      if ( alpha != 0.0 ) R *= pow( h, alpha-1.0 );
      if ( alpha != 0.0 ) r *= pow( h, alpha-1.0 );
      trace.info() << "- R=" << R << " r=" << r << " t=" << t << std::endl;
      VCMNormalEstimator estimator;
      estimator.attach( surface );
      estimator.setParams( embType, R, r, chi, t, Metric(), true );
      estimator.init( h, surface.begin(), surface.end() );
      trace.endBlock();
      computeEstimation( vm, K, shape, surface, estimator );
    }
  else if ( nameEstimator == "II" )
    {
      trace.beginBlock( "Chosen estimator is: II." );
      typedef typename KSpace::Space Space;
      typedef HyperRectDomain<Space> Domain;
      typedef ImageContainerBySTLVector< Domain, bool> Image;
      typedef typename Domain::ConstIterator DomainConstIterator;
      typedef SimpleThresholdForegroundPredicate<Image> ThresholdedImage;
      typedef IINormalDirectionFunctor<Space> IINormalFunctor;
      typedef IntegralInvariantCovarianceEstimator<KSpace, ThresholdedImage, IINormalFunctor> IINormalEstimator;
      double r = vm["r-radius"].as<double>();
      double alpha = vm["alpha"].as<double>();
      if ( alpha != 0.0 ) r *= pow( h, alpha-1.0 );
      trace.info() << " r=" << r << std::endl;
      trace.beginBlock( "Preparing characteristic set." );
      Domain domain( K.lowerBound(), K.upperBound() );
      Image image( domain );
      for ( DomainConstIterator it = domain.begin(), itE = domain.end(); it != itE; ++it )
        {
          image.setValue( *it, ptPred( *it ) );
        }
      trace.endBlock();
      trace.beginBlock( "Initialize II estimator." );
      ThresholdedImage thresholdedImage( image, false );
      IINormalEstimator ii_estimator( K, thresholdedImage );
      ii_estimator.setParams( r );
      ii_estimator.init( h, surface.begin(), surface.end() );
      trace.endBlock();
      trace.endBlock();
      computeEstimation( vm, K, shape, surface, ii_estimator );
   }
  else if ( nameEstimator == "Trivial" )
    {
      trace.beginBlock( "Chosen estimator is: Trivial." );
      typedef HatFunction<double> Functor;
      typedef typename KSpace::Space Space;
      typedef typename KSpace::Surfel Surfel;
      typedef typename Surface::DigitalSurfaceContainer SurfaceContainer;
      typedef ExactPredicateLpSeparableMetric<Space,2> Metric;
      typedef ElementaryConvolutionNormalVectorEstimator< Surfel, CanonicSCellEmbedder<KSpace> > 
        SurfelFunctor;
//.........这里部分代码省略.........

/// Processes physics for all registered objects
void PhysicsManager::ProcessPhysics()
{
	if (physicsState->simulationPaused)
		return;
	/// Returns straight away if paused.
	if (paused)
		return;
	if (physicalEntities.Size() == 0)
		return;

	activeTriangles.Clear();

	time_t currentTime = Timer::GetCurrentTimeMs();
	time_t millisecondsSinceLastUpdate = currentTime - lastUpdate;
	lastUpdate = currentTime;
  //  std::cout<<"\nCurrent time: "<<currentTime<<" last update: "<<lastUpdate;

	/// Throw away time if we've got more than 1 second, since this assumes we're debugging
	if (millisecondsSinceLastUpdate > 100){
		if (millisecondsSinceLastUpdate > 1000)
			std::cout<<"\nPhysicsManager::Throwing away "<<millisecondsSinceLastUpdate / 1000<<" debugging seconds";
		millisecondsSinceLastUpdate = 100;
	}

	float totalTimeSinceLastUpdate = millisecondsSinceLastUpdate * 0.001f;
	/// Multiply the total time since last update with the simulation speed multiplier before actual calculations are begun.
	totalTimeSinceLastUpdate = totalTimeSinceLastUpdate * simulationSpeed;

	/// Just return if simulation speed decreases beyond 0.1%!
	if (simulationSpeed <= 0.0001f){
		return;
	}

	/// Debugging time statistics
	float messageProcessingTime = 0;
	float recalculatingPropertiesDuration = 0;
	float collissionProcessingFrameTime = 0;

	// Reset previous frame-times
	recalculatingPropertiesDuration = 0;
	float integration = 0;
	collissionProcessingFrameTime = 0;
	physicsMeshCollisionChecks = 0;

	// To be sent for Collision callback.
	List<Message*> messages;


	/// Do one process for each 10 ms we've gotten stored up
	/// Get sub-time to calculate.
	float dt = 0.010f * simulationSpeed;
	float timeDiff = dt;
	float timeInSecondsSinceLastUpdate = dt;

	static float timeRemainingFromLastIteration = 0.f;
	/// Add time from last iteration that wasn't spent (since only evaluating one physics step at a time, 10 ms default).
	float timeToIterate = totalTimeSinceLastUpdate + timeRemainingFromLastIteration;
	float stepSize = 0.010f;
	int steps = timeToIterate / stepSize + 0.5f;

	/// Use a new step size based on the amount of steps. This will hopefully vary between 5.0 and 15.0 then.
	float newStepSize = timeToIterate / steps;
	int newStepSizeMs = newStepSize * 1000;
	newStepSize = newStepSizeMs * 0.001f;
	if (newStepSize < 0.005f || newStepSize > 0.015f)
	{
		LogPhysics("Step size out of good range: "+String(newStepSize), WARNING);
		if (newStepSize < 0.f)
			return;
//		assert(False)
	}
//	assert(newStepSize > 0.005f && newStepSize < 0.015f);
	

//	if (steps < 1) // At least 1 physics simulation per frame, yo. Otherwise you get a 'stuttering' effect when some frames have movement and some don't.
//		steps = 1;
	float timeLeft = timeToIterate - steps * newStepSizeMs * 0.001f;
	/// Store time we won't simulate now.
	timeRemainingFromLastIteration = timeLeft;
//	std::cout<<"\nSteps: "<<steps;

	for(int i = 0; i < steps; ++i)
	{
		/// Set current time in physics for this frame. This time is not the same as real time.
		physicsNowMs += newStepSizeMs;
			
		/// Process estimators (if any) within all registered entities?
		int milliseconds = newStepSizeMs;
		for (int i = 0 ; i < physicalEntities.Size(); ++i)
		{
			Entity * entity = physicalEntities[i];
			List<Estimator*> & estimators = entity->physics->estimators;
			for (int j = 0; j < estimators.Size(); ++j)
			{
				Estimator * estimator = estimators[j];
				estimator->Process(milliseconds);
				if (entity->name == "ExplosionEntity")
					int lp = 5;
				// Recalculate other stuff too.
//.........这里部分代码省略.........

void computeEstimation
( const po::variables_map& vm,     //< command-line parameters
  const KSpace& K,                 //< cellular grid space
  const ImplicitShape& shape,      //< implicit shape "ground truth"
  const Surface& surface,          //< digital surface approximating shape
  TrueEstimator& true_estimator,   //< "ground truth" estimator
  Estimator& estimator )           //< an initialized estimator
{
  typedef typename Surface::ConstIterator ConstIterator;
  typedef typename Surface::Surfel Surfel;
  typedef typename Estimator::Quantity Quantity;
  typedef double Scalar;
  typedef DepthFirstVisitor< Surface > Visitor;
  typedef GraphVisitorRange< Visitor > VisitorRange;
  typedef typename VisitorRange::ConstIterator VisitorConstIterator;
  
  std::string fname = vm[ "output" ].as<std::string>();
  string nameEstimator = vm[ "estimator" ].as<string>();
  trace.beginBlock( "Computing " + nameEstimator + "estimations." );
  CountedPtr<VisitorRange> range( new VisitorRange( new Visitor( surface, *(surface.begin()) )) );
  std::vector<Quantity> n_estimations;
  estimator.eval( range->begin(), range->end(), std::back_inserter( n_estimations ) );
  trace.info() << "- nb estimations  = " << n_estimations.size() << std::endl;
  trace.endBlock();

  trace.beginBlock( "Computing ground truth." );
  range = CountedPtr<VisitorRange>( new VisitorRange( new Visitor( surface, *(surface.begin()) )) );
  std::vector<Quantity> n_true_estimations;
  true_estimator.eval( range->begin(), range->end(), std::back_inserter( n_true_estimations ) );
  trace.info() << "- nb estimations  = " << n_true_estimations.size() << std::endl;
  trace.endBlock();

  trace.beginBlock( "Correcting orientations." );
  ASSERT( n_estimations.size() == n_true_estimations.size() );
  for ( unsigned int i = 0; i < n_estimations.size(); ++i )
    if ( n_estimations[ i ].dot( n_true_estimations[ i ] ) < 0 )
      n_estimations[ i ] = -n_estimations[ i ];
  trace.endBlock();

  DGtal::GradientColorMap<double> grad( 0.0, 40.0 );
  // 0 metallic blue, 5 light cyan, 10 light green, 15 light
  // yellow, 20 yellow, 25 orange, 30 red, 35, dark red, 40- grey
  grad.addColor( DGtal::Color( 128, 128, 255 ) ); // 0
  grad.addColor( DGtal::Color( 128, 255, 255 ) ); // 5
  grad.addColor( DGtal::Color( 128, 255, 128 ) ); // 10
  grad.addColor( DGtal::Color( 255, 255, 128 ) ); // 15
  grad.addColor( DGtal::Color( 255, 255, 0   ) ); // 20
  grad.addColor( DGtal::Color( 255, 128, 0   ) ); // 25
  grad.addColor( DGtal::Color( 255,   0, 0   ) ); // 30
  grad.addColor( DGtal::Color( 128,   0, 0   ) ); // 35
  grad.addColor( DGtal::Color( 128, 128, 128 ) ); // 40

  if ( vm.count( "angle-deviation-stats" ) )
    {
      trace.beginBlock( "Computing angle deviation error stats." );
      std::ostringstream adev_sstr;
      adev_sstr << fname << "-" << nameEstimator << "-angle-deviation-" 
                << estimator.h() << ".txt"; 
      DGtal::Statistic<Scalar> adev_stat;
      unsigned int i = 0;
      range = CountedPtr<VisitorRange>( new VisitorRange( new Visitor( surface, *(surface.begin()) )) );
      for ( VisitorConstIterator it = range->begin(), itE = range->end(); it != itE; ++it, ++i )
        {
          Quantity n_est = n_estimations[ i ];
          Quantity n_true_est = n_true_estimations[ i ];
          Scalar angle_error = acos( n_est.dot( n_true_est ) );
          adev_stat.addValue( angle_error );
        }
      adev_stat.terminate();
      std::ofstream adev_output( adev_sstr.str().c_str() );
      adev_output << "# Average error X of the absolute angle between two vector estimations." << std::endl;
      adev_output << "# h L1 L2 Loo E[X] Var[X] Min[X] Max[X] Nb[X]" << std::endl;
      adev_output << estimator.h() 
                  << " " << adev_stat.mean() // L1
                  << " " << sqrt( adev_stat.unbiasedVariance()
                                  + adev_stat.mean()*adev_stat.mean() ) // L2
                  << " " << adev_stat.max() // Loo
                  << " " << adev_stat.mean() // E[X] (=L1)
                  << " " << adev_stat.unbiasedVariance() // Var[X]
                  << " " << adev_stat.min() // Min[X]
                  << " " << adev_stat.max() // Max[X]
                  << " " << adev_stat.samples() // Nb[X]
                  << std::endl;
      adev_output.close();
      trace.endBlock();
    }
  if ( vm[ "export" ].as<string>() != "None" )
    {
      trace.beginBlock( "Exporting cell geometry." );
      std::ostringstream export_sstr;
      export_sstr << fname << "-" << nameEstimator << "-cells-" 
                  << estimator.h() << ".txt"; 
      std::ofstream export_output( export_sstr.str().c_str() );
      export_output << "# ImaGene viewer (viewSetOfSurfels) file format for displaying cells." << std::endl;
      bool adev =  vm[ "export" ].as<string>() == "AngleDeviation";
      unsigned int i = 0;
      range = CountedPtr<VisitorRange>( new VisitorRange( new Visitor( surface, *(surface.begin()) )) );
      for ( VisitorConstIterator it = range->begin(), itE = range->end(); it != itE; ++it, ++i )
        {
          Quantity n_est = n_estimations[ i ];
//.........这里部分代码省略.........

void runbenchmark(const vector<int>& Ls, const Pulse& tx, function<Estimator(vector<int>&,vector<int>&,vector<complex<double>>&)> estf, string name) {

  default_random_engine generator; 
      //SNRs
  double SNRdB = 10.0;
  double SNR = pow(10.0, SNRdB/10.0);
  normal_distribution<double> noise(0.0,T/Ts/SNR/2);
  uniform_int_distribution<int> unifM(1, M); //for generating M-PSK symbols
  
  vector<double> times; //array to store output
  
  for(auto L : Ls) {

    const unsigned int numpilots = L/5; //about 10% pilots 
    //symbol position setup
    vector<int> P;
    for(int i = 0; i < numpilots; i++) P.push_back(i); //pilots at the front
    vector<int> D;
    for(int i = numpilots; i < L; i++) D.push_back(i); //data at the back
    vector<complex<double>> s;
    for (int m = 1; m <= L; m++) s.push_back(polar<double>(1.0, 2 * pi * unifM(generator) / M));
    vector<complex<double>> pilots;
    for (int m = 0; m < numpilots; m++) pilots.push_back(s[m]);

    //construct the estimator we will run
    Estimator est = estf(P,D,pilots);
   
    //number of samples
    unsigned int N = (unsigned int) ceil((T*(L+40)+taumax)/Ts); //number of samples
    const complex<double> a0 = polar<double>(1,0.1*pi); //phase and amplitude
    
    //transmitted signal
    auto x = [&s,&tx,T,L] (double t) {
      int mini = max(1, (int)ceil((t - tx.tmax())/T));
      int maxi = min(L, (int)floor((t - tx.tmin())/T));
      complex<double> sum(0,0);
      for(int i = mini; i <= maxi; i++) sum += s[i-1] * tx.pulse(t - i*T);
      return sum;
    };

    //sampled received signal
    vector<complex<double>> r;
    for(int n = 1; n <= N; n++) r.push_back(a0*x(n*Ts-tau0) + complex<double>(noise(generator), noise(generator)));

    cout << "Benchmarking " << name << " L = " << L << " ... ";
    long iters = 0;
    double errmse = 0.0;
    clock_t started = clock();
    while( ((double)(clock() - started))/CLOCKS_PER_SEC < benchtime) {
      double tauhat = est.estimate(r);
      errmse += (tauhat - tau0)*(tauhat - tau0); //use tauhat otherwise it might get compiled out!
      iters++;
    }
    clock_t stopped = clock();
    double microsecs = ((double)(stopped - started))/CLOCKS_PER_SEC/iters/L*1000000;
    times.push_back(microsecs);
    cout << " requires  " << microsecs << " microseconds per symbol with average error " << (errmse/iters) << endl;
  }

  ofstream file(string("data/") + name + string("bench"));
  for(int i = 0; i < Ls.size(); i++) file << Ls[i] << "\t" << times[i] << endl;
  file.close();

}

int main(int argc, char** argv)
{
	char configFilename[256];
	
	if (argc > 1)
		strncpy(configFilename, argv[1], 255);
	else
		strcpy(configFilename, "conf.txt");

	SimulatorArgument simArgs;
	simArgs.load(configFilename);

	// real plane should be saved as plane.auto.conf, but for convenice, save plane.detected.conf
	// this will be rewrite after detecting plane process
	simArgs.planes.save("plane.detected.conf");
	simArgs.beacons.save("beacon.auto.conf");

	estArgs.load(configFilename);
	estArgs.estimatorMode = EST::TRADITIONAL;
	estArgs.optimization = 0;
	estimator.setEstimator(&estArgs);

	

	PlaneList realPlanes;

	PlaneList detectedPlanes;

	realPlanes.makeCube(simArgs.width, simArgs.length, simArgs.height);
	// there is 6 planes in realPlanes.
	// index 0~3 are side wall, 4 and 5 are ceiling and floor
	

	printf("Generating direct signal events...");
	fflush(stdout);
	// events for directly receiving signals
	EventGenerator directEventGenerator;
	directEventGenerator.generateEventForPlaneDetection(
			&simArgs, listenerInterval, minMargin, listenerZ, Vector(0, 0, 1));
	printf("done\n");

	Vector bestLocation[4];
	Vector bestLocationSimulated[4];
	double error[4];

	
	for (int i = 0; i < 4; i++)
	{
		Plane detectedPlane = detectPlane(
				&simArgs, 
				realPlanes.at(i), 
				i/*pid*/, 
				&directEventGenerator.events,
				&bestLocation[i],
				&bestLocationSimulated[i],
				&error[i]);
		detectedPlane.setBoundary(true);
		detectedPlanes.addPlane(detectedPlane);
		printf("min error : %f\n", error[i]);
	}

	for (int i = 0; i < 5; i++)
	{
		// make test plane list. this list has mixed set of real plane and detected plane
		PlaneList testPlanes;
		for (int j = 0; j < 4; j++)
		{
			if (i > j)			
				testPlanes.addPlane(*detectedPlanes.at(j));
			else
				testPlanes.addPlane(*realPlanes.at(j));
		}

		testPlanes.addPlane(*realPlanes.at(4));
		testPlanes.addPlane(*realPlanes.at(5));

		char filename[255];
		sprintf(filename, "plane.detected.%d.conf", i);
		
		testPlanes.save(filename);	
	}

		

	printf("\nResults");

	for (int i = 0; i < 4; i++)
	{
		printf("\n");
		printf("detected plane            : ");
		detectedPlanes.at(i)->println();

		
		printf("best location             : ");
		bestLocation[i].println();

		printf("best location (simulated) : ");
		bestLocationSimulated[i].println();

		printf("error                     : %f\n", error[i]);
//.........这里部分代码省略.........

//.........这里部分代码省略.........
        Vext.updatePlaneWaves();
        H.update(Vext);
    
        // SHAKE it for the correct lambda
        VectorType uc; // uncontraint c
        uc = 2*c-oldc-pow(dt,2)/emass*( (*H.myHam())*c -lambda*c );
        ComplexType usigma(-1.0,0.0);
        for (int i=0;i<uc.size();i++){
            usigma += conj( uc[i] )*uc[i];
        }
        ComplexType dot(0.0,0.0);
        for (int i=0;i<uc.size();i++){
            dot += conj( uc[i] )*conj( c[i] )/(ComplexType)emass;
        }
        lambda = usigma/dot;
        
        // move the electrons
        for (int i=0;i<uc.size();i++){
            c[i] = uc[i]-conj( c[i] )*lambda*(ComplexType)(dt/emass);
        }
        
        // report properties
        norm = c.norm();
        E = ( c.adjoint()*(*H.myHam())*c )(0,0).real();
        cout << "( " << E << "," << norm << ")" << endl; 
        
        if (abs(E-oldE)<thres) break;
        
        oldc = c; 
        oldE = E;
    }
    
    // ---------- we are now ready to do MD ----------
    
    // create lowest KS orbital and its associated density
    Function waveFunction(&waveFunctionBasis);
    Density density(&densityBasis);
    
    // build a force field (need to be based on the electron wave function)
    ForceField* ff;
    ff = new ForceField(&gPset,&density,&Vext,0.01);
    
    // use VelocityVerlet updator using the force field
    VelocityVerlet updator(&gPset,ff); updator.h=dt;
    
    // throw in some estimators
    Estimator *kinetic;
    kinetic     = new KineticEnergyEstimator(gPset);
  
    cout << "Finished energy minimization, perturb the ion to new position " << endl; 
    gPset.ptcls[0]->r[1] = 0.05;
    cout << gPset.str() << endl;
    RealType Tn, Te; // kinetic energy of ions and electrons
    gPset.clearFile(traj);
    cout << "Starting molecular dynamics " << endl;
    cout << "(Total Energy,Ion Kenetic, Norm)" << endl; 
    for (int istep=0;istep<nstep;istep++){
        
        // save trajectory
        gPset.appendFile(traj);
    
        // update Hamiltonian
        Vext.updatePlaneWaves();
        H.update(Vext);
        
        // update density because it is used by the force field
        waveFunction.initCoeff(c);
        density.updateWithWaveFunction(waveFunction);
        
        // report properties
        norm = c.norm();
        Tn = kinetic->scalarEvaluate();
        E = ( c.adjoint()*(*H.myHam())*c )(0,0).real() + Tn;
        cout << "( " << E << "," << Tn << "," << norm << ")" << endl; 
        
        // move the ions first since this depend on old waveFunction
        updator.update(); // density is used in here by ForceField
    
        // SHAKE it for the correct lambda
        VectorType uc; // uncontraint c
        uc = 2*c-oldc-pow(dt,2)/emass*( (*H.myHam())*c -lambda*c );
        ComplexType usigma(-1.0,0.0);
        for (int i=0;i<uc.size();i++){
            usigma += conj( uc[i] )*uc[i];
        }
        ComplexType dot(0.0,0.0);
        for (int i=0;i<uc.size();i++){
            dot += conj( uc[i] )*conj( c[i] )/(ComplexType)emass;
        }
        lambda = usigma/dot;
        
        // move the electrons
        for (int i=0;i<uc.size();i++){
            c[i] = uc[i]-conj( c[i] )*lambda*(ComplexType)(dt/emass);
        }
        oldc = c;
        
    }
return 0;
}

int main(int argc, char** argv)
{
	ros::init(argc, argv, "vins_estimator");
	ros::NodeHandle n("~");
	ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);

	ros::Publisher pubLeftImage = n.advertise<sensor_msgs::Image>("/leftImage",1000);
	ros::Publisher pubRightImage = n.advertise<sensor_msgs::Image>("/rightImage",1000);

	if(argc != 3)
	{
		printf("please intput: rosrun vins kitti_odom_test [config file] [data folder] \n"
			   "for example: rosrun vins kitti_odom_test "
			   "~/catkin_ws/src/VINS-Fusion/config/kitti_odom/kitti_config00-02.yaml "
			   "/media/tony-ws1/disk_D/kitti/odometry/sequences/00/ \n");
		return 1;
	}

	string config_file = argv[1];
	printf("config_file: %s\n", argv[1]);
	string sequence = argv[2];
	printf("read sequence: %s\n", argv[2]);
	string dataPath = sequence + "/";

	readParameters(config_file);
	estimator.setParameter();
	registerPub(n);

	// load image list
	FILE* file;
	file = std::fopen((dataPath + "times.txt").c_str() , "r");
	if(file == NULL){
	    printf("cannot find file: %stimes.txt\n", dataPath.c_str());
	    ROS_BREAK();
	    return 0;          
	}
	double imageTime;
	vector<double> imageTimeList;
	while ( fscanf(file, "%lf", &imageTime) != EOF)
	{
	    imageTimeList.push_back(imageTime);
	}
	std::fclose(file);

	string leftImagePath, rightImagePath;
	cv::Mat imLeft, imRight;
	FILE* outFile;
	outFile = fopen((OUTPUT_FOLDER + "/vio.txt").c_str(),"w");
	if(outFile == NULL)
		printf("Output path dosen't exist: %s\n", OUTPUT_FOLDER.c_str());

	for (size_t i = 0; i < imageTimeList.size(); i++)
	{	
		if(ros::ok())
		{
			printf("\nprocess image %d\n", (int)i);
			stringstream ss;
			ss << setfill('0') << setw(6) << i;
			leftImagePath = dataPath + "image_0/" + ss.str() + ".png";
			rightImagePath = dataPath + "image_1/" + ss.str() + ".png";
			//printf("%lu  %f \n", i, imageTimeList[i]);
			//printf("%s\n", leftImagePath.c_str() );
			//printf("%s\n", rightImagePath.c_str() );

			imLeft = cv::imread(leftImagePath, CV_LOAD_IMAGE_GRAYSCALE );
			sensor_msgs::ImagePtr imLeftMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imLeft).toImageMsg();
			imLeftMsg->header.stamp = ros::Time(imageTimeList[i]);
			pubLeftImage.publish(imLeftMsg);

			imRight = cv::imread(rightImagePath, CV_LOAD_IMAGE_GRAYSCALE );
			sensor_msgs::ImagePtr imRightMsg = cv_bridge::CvImage(std_msgs::Header(), "mono8", imRight).toImageMsg();
			imRightMsg->header.stamp = ros::Time(imageTimeList[i]);
			pubRightImage.publish(imRightMsg);


			estimator.inputImage(imageTimeList[i], imLeft, imRight);
			
			Eigen::Matrix<double, 4, 4> pose;
			estimator.getPoseInWorldFrame(pose);
			if(outFile != NULL)
				fprintf (outFile, "%f %f %f %f %f %f %f %f %f %f %f %f \n",pose(0,0), pose(0,1), pose(0,2),pose(0,3),
																	       pose(1,0), pose(1,1), pose(1,2),pose(1,3),
																	       pose(2,0), pose(2,1), pose(2,2),pose(2,3));
			
			//cv::imshow("leftImage", imLeft);
			//cv::imshow("rightImage", imRight);
			//cv::waitKey(2);
		}
		else
			break;
	}
	if(outFile != NULL)
		fclose (outFile);
	return 0;
}