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++)
{
// 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]);
}
// add ceiling and floor
detectedPlanes.addPlane(*realPlanes.at(4));
detectedPlanes.addPlane(*realPlanes.at(5));
// save detected planes
detectedPlanes.save("plane.detected.conf");
}
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;
typedef LocalEstimatorFromSurfelFunctorAdapter< SurfaceContainer, Metric, SurfelFunctor, Functor>
NormalEstimator;
double t = vm["trivial-radius"].as<double>();
Functor fct( 1.0, t );
CanonicSCellEmbedder<KSpace> canonic_embedder( K );
SurfelFunctor surfelFct( canonic_embedder, 1.0 );
NormalEstimator estimator;
estimator.attach( surface );
estimator.setParams( Metric(), surfelFct, fct, t );
estimator.init( 1.0, surface.begin(), surface.end() );
trace.endBlock();
computeEstimation( vm, K, shape, surface, estimator );
}
}
/// 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.
entity->physics->UpdateProperties(entity);
// Re-calculate transform matrix, as it was probably affected..?
entity->RecalculateMatrix(Entity::ALL_PARTS);
if (estimator->finished)
{
estimators.RemoveIndex(j, ListOption::RETAIN_ORDER);
--j;
delete estimator;
}
}
}
/// Awesome.
Integrate(newStepSize);
/// Apply external constraints
// ApplyContraints();
/// Apply pathfinding for all relevant entities - should be done in separate property-files. Or in more dedicated classes for specific games.
// ApplyPathfinding();
Timer collisionTimer;
collisionTimer.Start();
Timer timer;
timer.Start();
/// Detect collisions.
List<Collision> collisions;
Timer sweepTimer;
sweepTimer.Start();
// Generate pair of possible collissions via some optimized way (AABB-sorting or Octree).
List<EntityPair> pairs = this->aabbSweeper->Sweep();
sweepTimer.Stop();
int sweepDur = sweepTimer.GetMs();
FrameStats.physicsCollisionDetectionAABBSweep += sweepDur;
// std::cout<<"\nAABB sweep pairs: "<<pairs.Size()<<" with "<<physicalEntities.Size()<<" entities";
Timer detectorTimer;
detectorTimer.Start();
if (collisionDetector)
{
collisionDetector->DetectCollisions(pairs, collisions);
}
// Old approach which combined collision-detection and resolution in a big mess...
else
DetectCollisions();
detectorTimer.Stop();
int detectorMs = detectorTimer.GetMs();
FrameStats.physicsCollisionDetectionChosenDetector += detectorMs;
timer.Stop();
int thisFrame = timer.GetMs();
FrameStats.physicsCollisionDetection += thisFrame;
timer.Start();
/// And resolve them.
if (collisionResolver)
collisionResolver->ResolveCollisions(collisions);
timer.Stop();
FrameStats.physicsCollisionResolution += timer.GetMs();
timer.Start();
messages.Clear();
for (int i = 0; i < collisions.Size(); ++i)
{
Collision & c = collisions[i];
if (c.one->physics->onCollision)
c.one->OnCollision(c);
if (c.two->physics->onCollision)
c.two->OnCollision(c);
if (c.one->physics->collisionCallback || c.two->physics->collisionCallback)
{
/// Check max callbacks.
int & maxCallbacks1 = c.one->physics->maxCallbacks;
int & maxCallbacks2 = c.two->physics->maxCallbacks;
if (maxCallbacks1 == 0 || maxCallbacks2 == 0)
continue;
CollisionCallback * cc = new CollisionCallback(c.one, c.two);
cc->impactNormal = c.collisionNormal;
messages.Add(cc);
if (maxCallbacks1 > 0)
--maxCallbacks1;
if (maxCallbacks2 > 0)
--maxCallbacks2;
}
}
if (messages.Size())
MesMan.QueueMessages(messages);
timer.Stop();
FrameStats.physicsCollisionCallback += timer.GetMs();
collisionTimer.Stop();
FrameStats.physicsCollisions += collisionTimer.GetMs();
int64 colMs = collisionTimer.GetMs();
if (colMs > 50)
{
std::cout<<"\nCollision detection and resolution taking "<<colMs<<" milliseconds per frame.";
break; // Break the loop. Simulate more next time if it's already being slow.
}
}
// Recalc matrices for the semi-dynamic ones.
if (physicsIntegrator)
physicsIntegrator->RecalculateMatrices(semiDynamicEntities);
else
LogPhysics("No integrator assigned", ERROR);
// Reset previous frame-times
// FrameStats.physicsIntegration = integration;
collissionProcessingFrameTime = 0;
physicsMeshCollisionChecks = 0;
}
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 ];
Quantity n_true_est = n_true_estimations[ i ];
Scalar angle_error = acos( n_est.dot( n_true_est ) )*180.0 / 3.14159625;
Surfel s = *it;
export_output
<< "CellN"
<< " " << min( 1023, max( 512+K.sKCoord( s, 0 ), 0 ) )
<< " " << min( 1023, max( 512+K.sKCoord( s, 1 ), 0 ) )
<< " " << min( 1023, max( 512+K.sKCoord( s, 2 ), 0 ) )
<< " " << K.sSign( s );
Color c = grad( 0 );
if ( adev ) c = grad( max( 0.0, min( angle_error, 40.0 ) ) );
export_output << " " << ((double) c.red() / 255.0 )
<< " " << ((double) c.green() / 255.0 )
<< " " << ((double) c.blue() / 255.0 );
export_output << " " << n_est[ 0 ] << " " << n_est[ 1 ]
<< " " << n_est[ 2 ] << std::endl;
}
export_output.close();
trace.endBlock();
}
if ( vm.count( "normals" ) )
{
trace.beginBlock( "Exporting cells normals." );
std::ostringstream export_sstr;
export_sstr << fname << "-" << nameEstimator << "-normals-"
<< estimator.h() << ".txt";
std::ofstream export_output( export_sstr.str().c_str() );
export_output << "# kx ky kz sign n_est[0] n_est[1] n_est[2] n_true[0] n_true[1] n_true[2]" << std::endl;
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 ];
Surfel s = *it;
export_output
<< K.sKCoord( s, 0 ) << " " << K.sKCoord( s, 1 ) << " " << K.sKCoord( s, 2 )
<< " " << K.sSign( s )
<< " " << n_est[ 0 ] << " " << n_est[ 1 ] << " " << n_est[ 2 ]
<< " " << n_true_est[ 0 ] << " " << n_true_est[ 1 ] << " " << n_true_est[ 2 ]
<< std::endl;
}
export_output.close();
trace.endBlock();
}
if ( vm.count( "noff" ) )
{
trace.beginBlock( "Exporting NOFF file." );
std::ostringstream export_sstr;
export_sstr << fname << "-" << nameEstimator << "-noff-"
<< estimator.h() << ".off";
std::ofstream export_output( export_sstr.str().c_str() );
std::map<Surfel,Quantity> normals;
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 ];
normals[ *it ] = n_est;
}
CanonicSCellEmbedder<KSpace> surfelEmbedder( K );
typedef SCellEmbedderWithNormal< CanonicSCellEmbedder<KSpace> > Embedder;
Embedder embedder( surfelEmbedder, normals );
surface.exportAs3DNOFF( export_output, embedder );
export_output.close();
trace.endBlock();
}
#ifdef WITH_VISU3D_QGLVIEWER
if ( vm[ "view" ].as<string>() != "None" )
{
typedef typename KSpace::Space Space;
typedef Viewer3D<Space,KSpace> MyViewever3D;
typedef Display3DFactory<Space,KSpace> MyDisplay3DFactory;
int argc = 1;
char name[] = "Viewer";
char* argv[ 1 ];
argv[ 0 ] = name;
Surfel s;
QApplication application( argc, argv );
MyViewever3D viewer( K );
viewer.show();
viewer << SetMode3D( s.className(), "Basic" );
trace.beginBlock( "Viewing surface." );
bool adev = vm[ "view" ].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 ];
Quantity n_true_est = n_true_estimations[ i ];
Scalar angle_error = acos( n_est.dot( n_true_est ) )*180.0 / 3.14159625;
s = *it;
Color c = grad( 0 );
if ( adev ) c = grad( max( 0.0, min( angle_error, 40.0 ) ) );
viewer.setFillColor( c );
MyDisplay3DFactory::drawOrientedSurfelWithNormal( viewer, s, n_est, false );
}
trace.endBlock();
viewer << MyViewever3D::updateDisplay;
application.exec();
}
#endif
}
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)
{
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;
}
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]);
}
// add ceiling and floor
detectedPlanes.addPlane(*realPlanes.at(4));
detectedPlanes.addPlane(*realPlanes.at(5));
// save detected planes
detectedPlanes.save("plane.detected.conf");
}
int main(int argc, char* argv[]){
// gather inputs
if (argc<2){
cout << "please specify input" << endl;
exit(0);
}
InputManager manager(argv[1]);
int nstep = atoi( manager["CPMD"]["nstep"].c_str() );
double dt = atof( manager["CPMD"]["dt"].c_str() );
double emass = atof( manager["CPMD"]["emass"].c_str() );
double thres = atof( manager["CPMD"]["conv_thr"].c_str() );
double L = atof( manager["DFT"]["L"].c_str() );
double Ecut = atof( manager["DFT"]["Ecut"].c_str() );
int nbasis = atoi( manager["DFT"]["maxBasis"].c_str() );
int nx = atoi( manager["DFT"]["ngrid"].c_str() );
string traj=manager["output"]["trajectory"]; // Output files
// Put a hydrogen ion at the origin
ParticlePool pPool(1);
ParticleSet gPset(pPool.myParticles());
gPset.ptcls[0]->name="H";
gPset.ptcls[0]->m=1836;
gPset.ptcls[0]->q=1;
cout << gPset.str() << endl;
// choose a basis set
BasisSet waveFunctionBasis(nbasis,L);
BasisSet densityBasis(pow(2,_DFT_DIM)*nbasis,L);
waveFunctionBasis.initPlaneWaves(Ecut);
densityBasis.initPlaneWaves(2*Ecut);
cout << "Number of Wave Function Basis: " << waveFunctionBasis.size() << endl;
cout << "Number of Density Basis: " << densityBasis.size() << endl;
// construct external potential and Hamiltonian
ExternalPotential Vext(&densityBasis,gPset);
Vext.initGrid(L,nx);
Vext.updatePlaneWaves(); // obtain expansion coefficients for Vext
Hamiltonian H(&waveFunctionBasis);
H.update(Vext); // obtain expansion coefficients for Hamiltonian
// diagonalize the Hamiltonian
Eigen::SelfAdjointEigenSolver<MatrixType> eigensolver(*H.myHam());
ComplexType lambda=eigensolver.eigenvalues()[0]; // Lagrange Multiplier
cout << "The lowest eigenvalue of H is: " << lambda.real() << endl;
VectorType c = eigensolver.eigenvectors().col(0);
// mess up the electronic ground state a little
RealType E,norm;
c = c.Random(c.size())/100.+c;
c /= c.norm();
E = ( c.adjoint()*(*H.myHam())*c )(0,0).real();
cout << "Energy after slight electronic perturbation: " << E << endl;
cout << "Starting energy minimization " << endl;
cout << "(Energy, Norm)" << endl;
// See that CP can bring back the ground state
VectorType oldc=c;
RealType oldE=E;
for (int istep=0;istep<nstep;istep++){
// update Hamiltonian
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;
}
