void SM_UpdateHistory2( ExtrinsicParameters &pose )
{
Vec3d center = pose.getTranslation();
// update each correspondence
for (int i=0; i<cq.size(); i++) {
if ( cq[i].age > 0 )
cq[i].age--;
if ( !cq[i].valid() ) {
cq[i].notseen++;
cq[i].line->status = UNKNOWN;
}
if ( cq[i].valid() ) {
Edge *line = cq[i].line;
Vec3d a = line->getA()-center;
Vec3d b = line->getB()-center;
EdgePlane selected_edgeplane = cq[i].eps[cq[i].eid];
EdgePlane edgeplane = EdgePlane( a, b, center, selected_edgeplane._cameraId, 0, 0 );
edgeplane.fromWorldFrameToCameraFrame( pose );
double angle = edgeplane.angle( selected_edgeplane );
if ( angle > toRadians( 5.0 ) ) {
SM_Clear(i);
cq[i].notseen++;
LOG(LEVEL_INFO, "[%d][%d] valid but too far (angle = %.3f deg.) [%d,%d]", i, cq[i].line->_id, toDegrees(angle), cq[i].age, cq[i].notseen);
cq[i].line->status = UNKNOWN;
} else {
cq[i].notseen = 0;
cq[i].age++;
LOG(LEVEL_INFO, "[%d][%d] valid and continues (angle = %.3f deg.) [%d,%d]", i, cq[i].line->_id, toDegrees(angle), cq[i].age, cq[i].notseen);
if ( cq[i].age > 4 )
cq[i].line->status = ACCEPTED;
}
}
if ( cq[i].notseen > 0 && cq[i].notseen < 4 ) {
int p = history[cq[i]._id].size();
if ( p-cq[i].notseen-1 >= 0 ) {
cq[i] = history[cq[i]._id][p-cq[i].notseen-1];
cq[i].notseen++;
LOG(LEVEL_INFO, "[%d][%d] not seen but recovered in history [%d,%d]", i, cq[i].line->_id, cq[i].age, cq[i].notseen);
cq[i].line->status = PENDING;
} else {
LOG(LEVEL_INFO, "[%d][%d] not seen and not recovered in history [%d,%d]", i, cq[i].line->_id, cq[i].age, cq[i].notseen);
cq[i].line->status = UNKNOWN;
}
}
if ( !cq[i].valid() && cq[i].notseen >= 4 ) {
SM_Clear(i);
cq[i].notseen++;
LOG(LEVEL_INFO, "[%d][%d] invalid and not seen for too long [%d,%d]", i, cq[i].line->_id, cq[i].age, cq[i].notseen);
cq[i].line->status = UNKNOWN;
}
}
}
// insert a correspondence in the queue
// avoid duplicates (each model line has at most one correspondence)
void SM_InsertItem( corresStatus status, Edge *line )
{
assert( line != NULL );
for (int i=0; i<cq.size(); i++) {
if ( cq[i].line->_id == line->_id ) {
LOG(LEVEL_INFO, "trying to insert line that already exists %d", line->_id);
assert(false);
}
}
cq.push_back( corres( line, EdgePlane(), status ) );
}
/* convert a list of 3D edges into edge planes
*/
void Frame::convert3DEdgesToEdgePlanes (int sensorId, edgePlaneVector edges, edgePlaneVector &edgeplanes, Vec3d center)
{
return;
// read the 3D edges and convert them into edge planes
int counter = 0;
for (int edgeId=0;edgeId<edges.size();edgeId++) {
EdgePlane edge = edges[edgeId];
EdgePlane edgeplane = EdgePlane(edge._a - center,edge._b - center,center,sensorId,edgeId,counter);
if (edgeplane.length() > 0) {
edgeplanes.push_back(edgeplane);
counter++;
}
}
}
/* return the i-th edge plane
*/
EdgePlane Frame::getEdgePlane (int id)
{
assert (id < n_edgeplanes_chained);
for (int i=0;i<_edgeplanes_chained.size();i++) {
for (int j=0;j<_edgeplanes_chained[i].size();j++) {
if (_edgeplanes_chained[i][j]._uid == id)
return _edgeplanes_chained[i][j];
}
}
printf("edgeplane not found for ID %d. Max ID = %d\n",id,n_edgeplanes_chained);
assert (false);
return EdgePlane();
}
bool MyGlWindow::update_correspondences()
{
ExtrinsicParameters original_pose = _camera->getPose();
Vec3d eps = get_expected_position(8);
Quaternion epr = get_expected_rotation();
//_camera->setTranslation( eps );
// update correspondences
SM_UpdateCorrespondences( );
printf("correspondences updated [%d]\n", SM_Size());
// enforce geometric constraints
SM_VerifyCorrespondences( _camera->getPose() );
int i,j,run;
// quick hack for video -- remove asap
/*for (i=0;i<pose_history.size();i++) {
if ( pose_history[i].id == frameId ) {
_camera->setPose( pose_history[i] );
break;
}
}
return true;*/
// end quick hack
//detect_edges();
//std::vector< intVector > edges_buckets;
//distribute_edgeplanes_into_buckets( edges_buckets );
//ExtrinsicParameters pose = original_pose;
write_correspondences( frameId );
ExtrinsicParameters best_pose = original_pose;
// keep only valid correspondences
CorrespondenceVector ds;
for (i=0;i<SM_Size();i++) {
if ( SM_Get(i).valid() && SM_Get(i).age >= 4 && SM_Get(i).eid != -1) {
Correspondence d;
d.first = SM_Get(i).line;
d.second = SM_Get(i).eps[SM_Get(i).eid];
ds.push_back( d );
}
}
int n = ds.size();
if ( n < 5 ) {
LOG(LEVEL_INFO, "too few correspondences to localize (%d).", n );
return false;
}
_n_correspondences = n;
LOG(LEVEL_INFO, "using %d correspondences for localization", ds.size());
double max_distance = 2 * _LUT.inchToModelUnit( mmToInch(_camera->_max_translation_speed) );
// RANSAC
_point_cloud.clear();
double best_penalty = 1E10;
int size = INIT_MIN_CORRESPONDENCES;
std::vector< ExtrinsicParameters > poses;
double wweight = 0.0;
//double nweight = 0.0;
Vec3d average_position = Vec3d(0,0,0);
for (run=0;run<6000;run++) {
_camera->setPose( original_pose );
CorrespondenceVector set;
intVector indices;
selectNRandomInt( size, ds.size(), indices );
for (i=0;i<indices.size();i++) {
Correspondence d = ds[indices[i]];
set.push_back( d );
}
refineCameraPoseFromNCorrespondences( set );
if ( len(_camera->getTranslation() - original_pose.getTranslation()) > max_distance )
continue;
_point_cloud.push_back( _camera->getTranslation() );
poses.push_back( _camera->getPose() );
// keep pose that scores the best
double penalty = 0.0;
int counter = 0;
Vec3d center = _camera->getTranslation();
for (i=0;i<n;i++) {
bool used = false;
for (j=0;j<indices.size();j++) {
if ( indices[j] == i ) {
used = true;
break;
}
}
if ( used )
continue;
counter++;
Edge *line = ds[i].first;
Vec3d a = line->getA()-center;
Vec3d b = line->getB()-center;
EdgePlane edgeplane = EdgePlane( a, b, center, 0, 0, 0 );
edgeplane.fromWorldFrameToCameraFrame( _camera->getPose() );
penalty += fabs( edgeplane.angle( ds[i].second ) );
}
if ( counter > 0 ) {
penalty /= counter;
} else {
continue;
}
if ( penalty > EPS ) {
// update the average
average_position += 1.0 / penalty * _camera->getTranslation();
wweight += 1.0 / penalty;
//printf("weight = %f\n", wweight);
}
if ( penalty < best_penalty ) {
best_penalty = penalty;
best_pose = _camera->getPose();
}
//_camera->getPose().print();
}
_camera->setPose( best_pose );
//if ( wweight > EPS ) {
// printf("weight = %f\n", wweight);
// _camera->setTranslation( average_position / wweight );
//}
double dd = len(_camera->getTranslation() - original_pose.getTranslation());
if ( dd > max_distance ) {
_camera->setPose( original_pose );
return true;
}
return true;
}
// relock
void MyGlWindow::relock ( double max_dihedral_angle, int nruns )
{
// lookup LUT
_LUT.lookup( _camera->came.getTranslation(), MIN_SUBTENDED_ANGLE, 0.0, 100, 0);
int i;
printf("detecting edges\n");
// detect edges
detect_edges();
// distribute visible edges into buckets
std::vector< intVector > edges_buckets;
distribute_edgeplanes_into_buckets( edges_buckets );
// compute the region size on the tessellatio
printf("computing region\n");
int level = 1 + max_dihedral_angle / SPHERE_TESSELLATION_RESOLUTION;
LOG(LEVEL_INFO, "region size: %d", level);
int N = _LUT._lines.size();
printf("converting lines\n");
// convert the model lines into edgeplanes in the camera coordinate frame
edgePlaneVector lines_edgeplanes;
intVector line_ids;
for (i=0;i<N;i++) {
Vec3d a = _camera->fromWorldFrameToCameraFrame( _LUT._lines[i]->getA() );
Vec3d b = _camera->fromWorldFrameToCameraFrame( _LUT._lines[i]->getB() );
Vec3d s = Vec3d(0,0,0);
lines_edgeplanes.push_back( EdgePlane( a, b, s, -1, _LUT._lines[i]->_id, i ) );
line_ids.push_back( _LUT._lines[i]->_id );
}
corresVector correspondences;
// clear the state machine
SM_Clear();
// for each model line, search the possible matches
for (i=0;i<N;i++) {
EdgePlane line = lines_edgeplanes[i];
intVector cells;
get_edgeplane_buckets( line, cells );
// create a correspondence
corres c ( _LUT._lines[i] );
for (int m=0;m<cells.size();m++) {
int bucket_id = cells[m];
for (int j=0;j<edges_buckets[bucket_id].size();j++) {
int edge_id = edges_buckets[bucket_id][j];
EdgePlane edge;
if (!_camera->_frame.get_edgeplane_chained( edge_id, edge ) ) {
LOG(LEVEL_ERROR, "error accessing edgeplane %d out of %d edgeplanes.", edge_id, _camera->_frame.n_edgeplanes_chained );
continue;
}
double angle = line.angle( edge );
if ( angle > max_dihedral_angle )
continue;
c.eps.push_back( edge );
}
if ( c.valid() ) {
correspondences.push_back( c );
SM_InsertItem( c );
}
}
}
ExtrinsicParameters original_pose = _camera->getPose();
ExtrinsicParameters best_pose = original_pose;
double best_score = score_camera_pose( original_pose, edges_buckets );
LOG(LEVEL_INFO, "start score: %f", best_score );
// find the best camera pose possible
for (int run=0; run < nruns; run++) {
// reset camera pose
_camera->setPose( original_pose );
// draw random correspondences
CorrespondenceVector cs;
intVector indices;
selectNRandomInt( 5/*INIT_MIN_CORRESPONDENCES*/, SM_Size(), indices);
// for each correspondence, pick a choice randomly
for (i=0;i<indices.size();i++) {
Correspondence d;
d.first = SM_Get(indices[i]).line;
int p = MIN( SM_Get(indices[i]).eps.size()-1, (double)rand() / (RAND_MAX+1) * (SM_Get(indices[i]).eps.size()-1));
d.second = SM_Get(indices[i]).eps[p];
cs.push_back( d );
}
refineCameraPoseFromNCorrespondences( cs );
double score = score_camera_pose( _camera->getPose(), edges_buckets );
if ( score > best_score ) {
best_score = score;
best_pose = _camera->getPose();
}
}
// keep the best camera pose found so far
_camera->setPose( best_pose );
LOG(LEVEL_INFO, "new score: %f", best_score );
// clear the state machine
SM_Clear();
// populate correspondences again
correspondences.clear();
init_correspondences( correspondences, edges_buckets, MAINTENANCE_DIHEDRAL_ANGLE, MAINTENANCE_MIN_OVERLAP, true );
}
// populate a set of correspondences
// correspondences are saved in the state machine if <store> is set to true
//
void MyGlWindow::init_correspondences( corresVector &correspondences, std::vector< intVector > &edges_buckets, double max_dihedral_angle, double min_overlap, bool store )
{
int i,k, counter=0;
// lookup LUT
_LUT.lookup( _camera->came.getTranslation(), MIN_SUBTENDED_ANGLE, 0.0, 100, 0);
int N = MIN(MAINTENANCE_MAX_CORRESPONDENCES ,_LUT._lines.size());
int M = _camera->_frame.nedgeplanes_chained();
LOG(LEVEL_INFO, "init correspondences: %d model lines and %d images edges", N, M);
if ( store ) {
SM_Clear();
SM_ClearHistory();
}
correspondences.clear();
// convert the model lines into edgeplanes in the camera coordinate frame
edgePlaneVector lines_edgeplanes;
intVector line_ids;
for (i=0;i<N;i++) {
Vec3d a = _camera->fromWorldFrameToCameraFrame( _LUT._lines[i]->getA() );
Vec3d b = _camera->fromWorldFrameToCameraFrame( _LUT._lines[i]->getB() );
Vec3d s = Vec3d(0,0,0);
lines_edgeplanes.push_back( EdgePlane( a, b, s, -1, _LUT._lines[i]->_id, i ) );
line_ids.push_back( _LUT._lines[i]->_id );
}
std::vector< intVector > matches; // each element is a list of edge IDs matching the corresponding line
for (i=0;i<N;i++) { // the first element is the local line ID
intVector v;
v.push_back( i );
matches.push_back( v );
}
// for each model line, search the possible matches
for (i=0;i<N;i++) {
EdgePlane line = lines_edgeplanes[i];
intVector bucket_ids;
get_edgeplane_buckets( line, bucket_ids );
// create a correspondence
corres c ( _LUT._lines[i] );
c.age = 11;
c.line->status = UNKNOWN; // status of line is unknown
double best_angle = M_PI;
bool found = false;
EdgePlane best_edge;
for (k=0;k<bucket_ids.size();k++) {
int bucket_id = bucket_ids[k];
//LOG(LEVEL_INFO, "processing line %d (bucket: %d, has %d elements)", _LUT._lines[i]->_id, bucket_id, edges_buckets[bucket_id].size() );
for (int j=0;j<edges_buckets[bucket_id].size();j++) {
int edge_id = edges_buckets[bucket_id][j];
EdgePlane edge;
if (!_camera->_frame.get_edgeplane_chained( edge_id, edge ) ) {
LOG(LEVEL_ERROR, "error accessing edgeplane %d out of %d edgeplanes.", edge_id, _camera->_frame.n_edgeplanes_chained );
continue;
}
double angle = line.angle( edge );
if ( edge.overlap( line ) < EPS && line.overlap( edge ) < EPS )
continue;
if ( edge.length() < toRadians(15.0) )
continue;
if ( angle < best_angle && angle < toRadians( 5.0 ) ) {
best_angle = angle;
best_edge = edge;
found = true;
}
//c.eps.push_back( edge );
}
}
if ( found ) {
//LOG(LEVEL_INFO, "found a match");
_camera->updateColor( best_edge, false );
c.eps.push_back( best_edge );
c.eid = 0;
c.length = best_edge.length();
c.line->status = ACCEPTED;
counter++;
} else {
//LOG(LEVEL_INFO, "found no match");
}
correspondences.push_back( c );
if ( store )
SM_InsertItem( c );
}
// contraint geometry on correspondences
//SM_VerifyCorrespondences( _camera->getPose() );
ExtrinsicParameters pose = _camera->getPose();
pose.id = frameId;
LOG(LEVEL_INFO, "%d valid correspondences.", counter);
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
pose_history.push_back( pose );
}
template<class T> bool Polygon3T<T>::IsValid(const double RoundEpsilon, const double MinVertexDist) const
{
// Punkt 4: Es muß mindestens 3 Vertices geben.
if (Vertices.Size()<3) return false;
// Punkt 4: Die Plane muß halbwegs wohldefiniert sein.
if (!Plane.IsValid()) return false;
// Ohne expliziten Punkt: Alle Vertices müssen *in* der Ebene liegen.
for (unsigned long VertexNr=0; VertexNr<Vertices.Size(); VertexNr++)
{
const double Dist=Plane.GetDistance(Vertices[VertexNr]);
if (Dist> RoundEpsilon) return false;
if (Dist<-RoundEpsilon) return false;
}
// Punkt 5: No two vertices must be coincident.
// Normally I'd only run the check for all (V[i], V[i+1]) pairs, but even with the additional check for
// convexity below, that would not catch rhombuses (Rauten) whose top and bottom vertices are near identical,
// that is, closer than MinVertexDist.
for (unsigned long VertexNr=0; VertexNr+1<Vertices.Size(); VertexNr++)
for (unsigned long VNr=VertexNr+1; VNr<Vertices.Size(); VNr++)
if (length(Vertices[VertexNr]-Vertices[VNr])<MinVertexDist) return false;
// Punkt 1 und Punkt 3: Stelle Konvexität und Orientierung sicher.
for (unsigned long VertexNr=0; VertexNr<Vertices.Size(); VertexNr++)
{
// Intentionally don't cache any DivisionByZero exceptions here.
// If one occurs, the situation is much worse than a "return false;" would express
// (the EdgePlane ctor *must* succeed after all the assertions above).
unsigned long NextVertexNr=VertexNr+1<Vertices.Size() ? VertexNr+1 : 0;
Plane3T<T> EdgePlane(Vertices[VertexNr],
Vertices[NextVertexNr],
Vertices[VertexNr]-Plane.Normal, T(0.00001)); // (Assume that MinVertexDist is well larger than 1.)
// Alle Vertices des Polys außer VertexNr und NextVertexNr müssen mindestens RoundEpsilon über der EdgePlane liegen!
for (unsigned long VNr=0; VNr<Vertices.Size(); VNr++)
if (VNr!=VertexNr && VNr!=NextVertexNr)
if (EdgePlane.GetDistance(Vertices[VNr])<RoundEpsilon) return false;
}
// Punkt 2: Das Polygon befindet sich (bzgl. Orientierung) auf der *Vorderseite* der Ebene
// Konstruiere dazu einen Vektor, der auf den Vektoren der Edges 0-1 und 1-2 senkrecht steht,
// und projeziere diesen Vektor auf den Einheitsnormalenvektor der Poly.Plane.
const Vector3T<T> N_=cross(Vertices[1]-Vertices[0], Vertices[2]-Vertices[1]);
const T l_=length(N_);
// Wenn l_==0.0 tatsächlich vorkäme, wäre das ein schwerer Fehler!
// l_ darf nämlich sehr klein sein, aber nach obigen Tests keinesfalls 0.
if (l_==0.0) return false;
const T Proj=-dot(scale(N_, T(1.0)/l_), Plane.Normal);
if (Proj>1.0+RoundEpsilon) return false;
if (Proj<1.0-RoundEpsilon) return false;
// Alle Tests bestanden!
return true;
}
// report statistics
//
void SM_Statistics( int frameid, ExtrinsicParameters &pose )
{
int i;
// open the file
FILE *f = fopen( "loca_stats.dat", "a" );
if ( f == NULL ) {
LOG(LEVEL_ERROR, "could not open file loca_stats.dat!");
return;
}
// compute the distribution of correspondences
double accepted=0.0, pending=0.0, unknown=0.0;
int n = cq.size();
for (i=0; i<n; i++) {
if (cq[i].line == NULL)
continue;
if (cq[i].line->status == ACCEPTED)
accepted += 1.0 / n;
if (cq[i].line->status == PENDING)
pending += 1.0 / n;
if (cq[i].line->status == UNKNOWN)
unknown += 1.0 / n;
}
// compute the average and standard deviation of angular error for accepted correspondences
doubleVector angles;
Vec3d center = pose.getTranslation();
for (i=0; i<n; i++) {
if ( cq[i].line == NULL )
continue;
if ( cq[i].line->status != ACCEPTED )
continue;
Edge *line = cq[i].line;
Vec3d a = line->getA()-center;
Vec3d b = line->getB()-center;
EdgePlane selected_edgeplane = cq[i].eps[cq[i].eid];
EdgePlane edgeplane = EdgePlane( a, b, center, selected_edgeplane._cameraId, 0, 0 );
edgeplane.fromWorldFrameToCameraFrame( pose );
double angle = toDegrees( edgeplane.angle( selected_edgeplane ) );
angles.push_back( angle );
}
int n_accepted = angles.size();
double average = 0.0;
double stdev = 0.0;
if ( n_accepted > 0 ) {
for (i=0; i<angles.size(); i++)
average += angles[i] / n_accepted;
for (i=0; i<angles.size(); i++)
stdev += ( angles[i] - average ) * ( angles[i] - average );
stdev = sqrt( stdev ) / n_accepted;
}
// write the stats into the file
fprintf(f, "%d %d %.1f %.1f %.1f %.2f %.2f\n", frameid, n, 100.0 * accepted, 100.0 * pending, 100.0 * unknown, average, stdev);
fclose(f);
}
void SM_VerifyCorrespondences( ExtrinsicParameters &pose )
{
// 1. if two model lines get assigned to the same image edge, one correspondence must go
int i,j;
Vec3d center = pose.getTranslation();
// for each pair of valid correspondences
bool done = false;
while (!done) {
done = true;
for (i=0; i<cq.size(); i++) {
if ( !cq[i].valid() )
continue;
for (j=i+1; j<cq.size(); j++) {
if ( !cq[j].valid() )
continue;
// if the two correspondences have the same image edge...
if ( cq[i].eps[cq[i].eid]._uid == cq[j].eps[cq[j].eid]._uid ) {
done = false;
EdgePlane edge = cq[i].eps[cq[i].eid];
EdgePlane line_i, line_j;
Vec3d a = cq[i].line->getA()-center;
Vec3d b = cq[i].line->getB()-center;
// make a synthetic edge plane for model line 1
line_i = EdgePlane( a, b, center, 0, 0, 0 );
line_i.fromWorldFrameToCameraFrame( pose );
a = cq[j].line->getA()-center;
b = cq[j].line->getB()-center;
// make a synthetic edge plane for model line 2
line_j = EdgePlane( a, b, center, 0, 0, 0 );
line_j.fromWorldFrameToCameraFrame( pose );
// cancel one of the two correspondences
if ( edge.angle( line_i ) < edge.angle( line_j ) ) {
cq[j].eps.clear();
cq[j].eid = -1;
cq[j].length = 0.0;
} else {
cq[i].eps.clear();
cq[i].eid = -1;
cq[i].length = 0.0;
}
break;
}
}
if ( !done )
break;
}
}
return;
// 2. if two close lines are assigned to two close edges, enforce ordering
done = false;
while (!done) {
done = true;
for (i=0; i<cq.size(); i++) {
if ( !cq[i].valid() )
continue;
for (j=i+1; j<cq.size(); j++) {
if ( !cq[j].valid() )
continue;
EdgePlane ei = cq[i].eps[cq[i].eid];
EdgePlane ej = cq[j].eps[cq[j].eid];
if ( ei.angle( ej ) > toRadians( 10.0 ) )
continue;
if ( ei.overlap( ej ) < EPS || ej.overlap( ei ) < EPS )
continue;
// flip normals if needed
if ( dot(ei._normal, ej._normal) < 0 )
ej._normal = -ej._normal;
EdgePlane line_i, line_j;
Vec3d a = cq[i].line->getA()-center;
Vec3d b = cq[i].line->getB()-center;
// make a synthetic edge plane for model line 1
line_i = EdgePlane( a, b, center, 0, 0, 0 );
line_i.fromWorldFrameToCameraFrame( pose );
a = cq[j].line->getA()-center;
b = cq[j].line->getB()-center;
// make a synthetic edge plane for model line 2
line_j = EdgePlane( a, b, center, 0, 0, 0 );
line_j.fromWorldFrameToCameraFrame( pose );
// check that the swap is possible
if ( ej.overlap( line_i ) < 0.5 || ei.overlap( line_j ) < 0.5 )
continue;
// flip normals if needed
if ( dot(line_i._normal, ei._normal) < 0 )
line_i._normal = -line_i._normal;
if ( dot(line_j._normal, ei._normal) < 0 )
line_j._normal = -line_j._normal;
// swap correspondences
if ( dot(cross(ei._normal,ej._normal), cross(line_i._normal, line_j._normal)) < 0.0 ) {
LOG(LEVEL_INFO, "flipping correspondences %d and %d", i, j);
cq[i].eps[cq[i].eid] = ej;
cq[j].eps[cq[j].eid] = ei;
}
}
}
}
}
