void MyGlWindow::refine_from_accepted_correspondences ()
{
ExtrinsicParameters original_pose = _camera->getPose();
double max_distance = _LUT.inchToModelUnit( mmToInch(_camera->_max_translation_speed) );
CorrespondenceVector ds;
for (int i=0;i<SM_Size();i++) {
if ( SM_Get(i).valid() && SM_Get(i).line->status == ACCEPTED && 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 < 15 ) {
return;
} else {
LOG(LEVEL_INFO, "refining from accepted correspondences (%d).", n );
}
refineCameraPoseFromNCorrespondences( ds );
if ( len( _camera->getTranslation() - original_pose.getTranslation()) > max_distance )
_camera->setPose( original_pose );
}
bool IdCorrespondenceValidator::operator()(const CorrespondenceVector& correspondences, const IndexVector& indices, int k){
if (k>minimalSetSize())
return true;
assert(indices.size()>=k && "VALIDATION_INDEX_OUT_OF_BOUND");
assert(correspondences.size()>=indices[k] && "VALIDATION_CORRESPONDENCE_INDEX_OUT_OF_BOUND");
const g2o::OptimizableGraph::Edge* edgek = correspondences[indices[k]].edge();
int idk1=edgek->vertex(0)->id();
int idk2=edgek->vertex(1)->id();
for (int i=0; i<k-1; i++){
const g2o::OptimizableGraph::Edge* edge = correspondences[indices[i]].edge();
int id1=edge->vertex(0)->id();
int id2=edge->vertex(1)->id();
if (idk1==id1)
return false;
if (idk2==id2)
return false;
}
return true;
}
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;
}
// ransac-based refine camera pose based on set of correspondences
//
double MyGlWindow::refine_camera_pose( corresVector &correspondences, std::vector< intVector > &edges_buckets )
{
ExtrinsicParameters best_pose = _camera->getPose();
double best_score = score_camera_pose( best_pose, edges_buckets );
int n = correspondences.size();
int i,j,k;
_line_matching_history.clear();
_line_matching_pose_history.clear();
if ( n < INIT_MIN_CORRESPONDENCES )
return best_score;
// for each run, draw a random subset of correspondences and compute the camera pose based on it
for (int run = 0; run < 1000; run++) {
CorrespondenceVector cs;
intVector indices;
selectNRandomInt( INIT_MIN_CORRESPONDENCES, n-1, indices ); // select random correspondences
for (i=0;i<indices.size();i++) {
corres c = correspondences[indices[i]];
int index = MIN(c.eps.size()-1, int( (double)rand()/(RAND_MAX+1) * (c.eps.size()-1) )); // pick a random edge in each correspondence
Correspondence d;
d.first = c.line;
d.second = c.eps[index];
cs.push_back( d );
}
if ( cs.size() < INIT_MIN_CORRESPONDENCES )
LOG(LEVEL_ERROR, "warning: number of correspondences mismatch: %d instead of %d", cs.size(), INIT_MIN_CORRESPONDENCES);
// compute the coarse camera pose from three lines
bool success = false;
for (i=0;i<cs.size();i++) {
for (j=i+1;j<cs.size();j++) {
for (k=j+1;k<cs.size();k++) {
ExtrinsicParameters pose = _camera->getPose();
success = cameraPoseFromThreeCorrespondences( cs[i].second, cs[j].second, cs[k].second, cs[i].first, cs[j].first, cs[k].first, pose );
if ( success ) {
_camera->setPose( pose );
break;
}
}
if ( success ) break;
}
if ( success ) break;
}
// update the history (for debugging only)
intVector lines_id_history;
edgePlaneVector edgeplanes_history;
for (k=0;k<cs.size();k++) {
lines_id_history.push_back( cs[k].first->_id );
edgeplanes_history.push_back( cs[k].second );
}
_line_matching_history.push_back( std::pair< intVector, edgePlaneVector > (lines_id_history, edgeplanes_history) );
// end debugging
find_inliers_and_refine_camera_pose( correspondences );
_line_matching_pose_history.push_back( _camera->getPose() );
double score = score_camera_pose( _camera->getPose(), edges_buckets );
if ( score > best_score ) {
if ( _camera->getTranslation()[2] > 0.0 && _camera->getTranslation()[2] < INIT_MAX_CAMERA_HEIGHT && _LUT.valid_pose( _camera->getPose() ) ) {
best_pose = _camera->getPose();
best_score = score;
}
}
}
// refine the translation
//refine_camera_translation( best_pose, _camera->getHeight()-_LUT.inchToModelUnit(20.0), _camera->getHeight()+_LUT.inchToModelUnit(20.0), edges_buckets);
// set camera pose to best solution found so far
_camera->setPose( best_pose );
return best_score;
}
