void field_t::get_empty_around(const point& c,points_t& res,int bound_size) const
{
res.resize(0);
for(int y=c.y-bound_size;y<=c.y+bound_size;y++)
for(int x=c.x-bound_size;x<=c.x+bound_size;x++)
{
point p(x,y);
if(at(p)==st_empty)
res.push_back(p);
}
std::sort(res.begin(),res.end(),less_point_pr());
res.erase(std::unique(res.begin(),res.end()),res.end());
}
void field_t::get_empty_around(points_t& res,int bound_size) const
{
res.resize(0);
for(unsigned i=0;i<steps.size();i++)
{
const step_t& st=steps[i];
for(int y=st.y-bound_size;y<=st.y+bound_size;y++)
for(int x=st.x-bound_size;x<=st.x+bound_size;x++)
{
point p(x,y);
if(at(p)==st_empty)
res.push_back(p);
}
}
std::sort(res.begin(),res.end(),less_point_pr());
res.erase(std::unique(res.begin(),res.end()),res.end());
}
void
opengv::generateRandom2D3DCorrespondences(
const translation_t & position,
const rotation_t & rotation,
const translations_t & camOffsets,
const rotations_t & camRotations,
size_t numberPoints,
double noise,
double outlierFraction,
bearingVectors_t & bearingVectors,
points_t & points,
std::vector<int> & camCorrespondences,
Eigen::MatrixXd & gt )
{
//initialize point-cloud
double minDepth = 4;
double maxDepth = 8;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
gt.col(i) = generateRandomPoint( maxDepth, minDepth );
//create the 2D3D-correspondences by looping through the cameras
size_t numberCams = camOffsets.size();
size_t camCorrespondence = 0;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
{
//get the camera transformation
translation_t camOffset = camOffsets[camCorrespondence];
rotation_t camRotation = camRotations[camCorrespondence];
//store the point
points.push_back(gt.col(i));
//project the point into the viewpoint frame
point_t bodyPoint = rotation.transpose()*(gt.col(i) - position);
//project the point into the camera frame
bearingVectors.push_back(camRotation.transpose()*(bodyPoint - camOffset));
//normalize the bearing-vector to 1
bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
//add noise
if( noise > 0.0 )
bearingVectors[i] = addNoise(noise,bearingVectors[i]);
//push back the camera correspondence
camCorrespondences.push_back(camCorrespondence++);
if(camCorrespondence > (numberCams-1) )
camCorrespondence = 0;
}
//add outliers
//compute the number of outliers based on fraction
size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
//make the first numberOutliers points be outliers
for(size_t i = 0; i < numberOutliers; i++)
{
//extract the camera transformation
translation_t camOffset = camOffsets[camCorrespondences[i]];
rotation_t camRotation = camRotations[camCorrespondences[i]];
//create random point
point_t p = generateRandomPoint(8,4);
//project into viewpoint frame
point_t bodyPoint = rotation.transpose()*(p - position);
//project into camera-frame and use as outlier measurement
bearingVectors[i] = camRotation.transpose()*(bodyPoint - camOffset);
//normalize the bearing vector
bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
}
}
inline void push_back( T x, T y ) { const point_t p(x,y); points.push_back(p); }