void
opengv::generateRandom3D3DCorrespondences(
const translation_t & position1,
const rotation_t & rotation1,
const translation_t & position2,
const rotation_t & rotation2,
size_t numberPoints,
double noise,
double outlierFraction,
bearingVectors_t & points1,
bearingVectors_t & points2,
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 3D-3D correspondences
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
{
//transform the points into the frames and store
points1.push_back(rotation1.transpose()*(gt.col(i) - position1));
points2.push_back(rotation2.transpose()*(gt.col(i) - position2));
//add noise
if( noise > 0.0 )
{
points1[i] = points1[i] + generateRandomTranslation(noise);
points2[i] = points2[i] + generateRandomTranslation(noise);
}
}
//add outliers
size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
for(size_t i = 0; i < numberOutliers; i++)
{
//generate a random point
point_t p = generateRandomPoint(8,4);
//push-back in frame 2 only to create outlier
points2[i] = rotation2.transpose()*(p - position2);
}
}
//null test
bool runKnnNullTests(){
SPBPQueue tempQueue = NULL;
SPPoint point = generateRandomPoint(4,1);
tempQueue = spBPQueueCreate(4);
kNearestNeighbors(NULL,tempQueue, point);
ASSERT_TRUE(spBPQueueIsEmpty(tempQueue));
spPointDestroy(point);
spBPQueueDestroy(tempQueue);
return true;
}
void
opengv::generateMulti2D2DCorrespondences(
const translation_t & position1,
const rotation_t & rotation1,
const translation_t & position2,
const rotation_t & rotation2,
const translations_t & camOffsets,
const rotations_t & camRotations,
size_t pointsPerCam,
double noise,
double outlierFraction,
std::vector<boost::shared_ptr<bearingVectors_t> > & multiBearingVectors1,
std::vector<boost::shared_ptr<bearingVectors_t> > & multiBearingVectors2,
std::vector<boost::shared_ptr<Eigen::MatrixXd> > & gt )
{
//initialize point-cloud
double minDepth = 4;
double maxDepth = 8;
for( size_t cam = 0; cam < camOffsets.size(); cam++ )
{
boost::shared_ptr<Eigen::MatrixXd> gt_sub(new Eigen::MatrixXd(3,pointsPerCam));
for( size_t i = 0; i < pointsPerCam; i++ )
gt_sub->col(i) = generateRandomPoint( maxDepth, minDepth );
gt.push_back(gt_sub);
}
//iterate through the cameras (pairs)
for( size_t cam = 0; cam < camOffsets.size(); cam++ )
{
//create the bearing-vector arrays for this camera
boost::shared_ptr<bearingVectors_t> bearingVectors1(new bearingVectors_t());
boost::shared_ptr<bearingVectors_t> bearingVectors2(new bearingVectors_t());
//get the offset and rotation of this camera
translation_t camOffset = camOffsets[cam];
rotation_t camRotation = camRotations[cam];
//now iterate through the points of that camera
for( size_t i = 0; i < (size_t) pointsPerCam; i++ )
{
//project a point into both viewpoint frames
point_t bodyPoint1 = rotation1.transpose()*(gt[cam]->col(i) - position1);
point_t bodyPoint2 = rotation2.transpose()*(gt[cam]->col(i) - position2);
//project that point into the cameras
bearingVectors1->push_back( camRotation.transpose()*(bodyPoint1 - camOffset) );
bearingVectors2->push_back( camRotation.transpose()*(bodyPoint2 - camOffset) );
//normalize the vectors
(*bearingVectors1)[i] = (*bearingVectors1)[i] / (*bearingVectors1)[i].norm();
(*bearingVectors2)[i] = (*bearingVectors2)[i] / (*bearingVectors2)[i].norm();
//add noise
if( noise > 0.0 )
{
(*bearingVectors1)[i] = addNoise(noise,(*bearingVectors1)[i]);
(*bearingVectors2)[i] = addNoise(noise,(*bearingVectors2)[i]);
}
}
//push back the stuff for this camera
multiBearingVectors1.push_back(bearingVectors1);
multiBearingVectors2.push_back(bearingVectors2);
}
//add outliers
size_t outliersPerCam = (size_t) floor(outlierFraction*pointsPerCam);
//iterate through the cameras
for(size_t cam = 0; cam < camOffsets.size(); cam++)
{
//get the offset and rotation of this camera
translation_t camOffset = camOffsets[cam];
rotation_t camRotation = camRotations[cam];
//add outliers
for(size_t i = 0; i < outliersPerCam; i++)
{
//generate a random point
point_t p = generateRandomPoint(8,4);
//transform that point into viewpoint 2 only
point_t bodyPoint = rotation2.transpose()*(p - position2);
//use as measurement (outlier)
(*multiBearingVectors2[cam])[i] =
camRotation.transpose()*(bodyPoint - camOffset);
//normalize
(*multiBearingVectors2[cam])[i] =
(*multiBearingVectors2[cam])[i] / (*multiBearingVectors2[cam])[i].norm();
}
}
}
void
opengv::generateRandom2D2DCorrespondences(
const translation_t & position1,
const rotation_t & rotation1,
const translation_t & position2,
const rotation_t & rotation2,
const translations_t & camOffsets,
const rotations_t & camRotations,
size_t numberPoints,
double noise,
double outlierFraction,
bearingVectors_t & bearingVectors1,
bearingVectors_t & bearingVectors2,
std::vector<int> & camCorrespondences1,
std::vector<int> & camCorrespondences2,
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];
//get the point in viewpoint 1
point_t bodyPoint1 = rotation1.transpose()*(gt.col(i) - position1);
//get the point in viewpoint 2
point_t bodyPoint2 = rotation2.transpose()*(gt.col(i) - position2);
//get the point in the camera in viewpoint 1
bearingVectors1.push_back(camRotation.transpose()*(bodyPoint1 - camOffset));
//get the point in the camera in viewpoint 2
bearingVectors2.push_back(camRotation.transpose()*(bodyPoint2 - camOffset));
//normalize the bearing-vectors
bearingVectors1[i] = bearingVectors1[i] / bearingVectors1[i].norm();
bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();
//add noise
if( noise > 0.0 )
{
bearingVectors1[i] = addNoise(noise,bearingVectors1[i]);
bearingVectors2[i] = addNoise(noise,bearingVectors2[i]);
}
//push back the camera correspondences
camCorrespondences1.push_back(camCorrespondence);
camCorrespondences2.push_back(camCorrespondence++);
if(camCorrespondence > (numberCams - 1) )
camCorrespondence = 0;
}
//add outliers
size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
for(size_t i = 0; i < numberOutliers; i++)
{
//get the corresponding camera transformation
translation_t camOffset = camOffsets[camCorrespondence];
rotation_t camRotation = camRotations[camCorrespondence];
//create random point
point_t p = generateRandomPoint(8,4);
//project this point into viewpoint 2
point_t bodyPoint = rotation2.transpose()*(p - position2);
//project this point into the corresponding camera in viewpoint 2
//and use as outlier
bearingVectors2[i] = camRotation.transpose()*(bodyPoint - camOffset);
//normalize the bearing vector
bearingVectors2[i] = bearingVectors2[i] / bearingVectors2[i].norm();
}
}
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();
}
}
//random tests
bool runRandomKnnTest(){
int maxDim, size, k;
bool successFlag = true;
SPPoint* pointsArray = NULL;
SPPoint queryPoint = NULL;
SPBPQueue queue = NULL;
SPKDArray kdArr = NULL;
SPKDTreeNode tree = NULL;
SP_KDTREE_SPLIT_METHOD splitMethod;
maxDim = 1 + (int)(rand() % RANDOM_TESTS_DIM_RANGE);
size = 2 + (int)(rand() % RANDOM_TESTS_SIZE_RANGE); //size = 1 is tested at a the edge cases
k = (int)(rand() % size);
splitMethod = (int)(rand()%3);
pointsArray = generateRandomPointsArray(maxDim,size);
queryPoint = generateRandomPoint(maxDim, size+1);
queue = spBPQueueCreate(k);
if (queue == NULL){
destroyCaseData(tree,kdArr,pointsArray,size,queue,queryPoint);
spLoggerSafePrintError(COULD_NOT_INITIALIZE_QUERY_POINT,
__FILE__, __FUNCTION__,
__LINE__);
FAIL(COULD_NOT_INITIALIZE_QUERY_POINT);
return false;
}
if (queryPoint == NULL){
destroyCaseData(tree,kdArr,pointsArray,size,queue,queryPoint);
spLoggerSafePrintError(COULD_NOT_INITIALIZE_QUERY_POINT,
__FILE__, __FUNCTION__,
__LINE__);
FAIL(COULD_NOT_INITIALIZE_QUERY_POINT);
return false;
}
if (pointsArray == NULL){
destroyCaseData(tree,kdArr,pointsArray,size,queue,queryPoint);
spLoggerSafePrintError(COULD_NOT_CREATE_POINTS_ARRAY,
__FILE__, __FUNCTION__,
__LINE__);
FAIL(COULD_NOT_CREATE_POINTS_ARRAY);
return false;
}
kdArr = Init(pointsArray,size);
if (kdArr == NULL){
destroyCaseData(tree,kdArr,pointsArray,size,queue,queryPoint);
spLoggerSafePrintError(COULD_NOT_INITIALIZE_KD_ARRAY,
__FILE__, __FUNCTION__,
__LINE__);
FAIL(COULD_NOT_INITIALIZE_KD_ARRAY);
return false;
}
tree = InitKDTree(kdArr, splitMethod);
if (tree == NULL){
destroyCaseData(tree,kdArr,pointsArray,size,queue,queryPoint);
spLoggerSafePrintError(
COULD_NOT_INITIALIZE_TREE,
__FILE__, __FUNCTION__,
__LINE__);
FAIL(COULD_NOT_INITIALIZE_TREE);
return false;
}
successFlag = kNearestNeighbors(tree, queue, queryPoint);
if (!successFlag){
destroyCaseData(tree,kdArr,pointsArray,size,queue,queryPoint);
return false;
}
successFlag = verifyKNN(queue, k, pointsArray,queryPoint,size);
destroyCaseData(tree,kdArr,pointsArray,size,queue,queryPoint);
return successFlag;
}