double
opengv::absolute_pose::modules::gpnp_evaluate(
const Eigen::Matrix<double,12,1> & solution,
const points_t & c,
translation_t & t,
rotation_t & R )
{
points_t ccam;
for(size_t i = 0; i<4; i++)
ccam.push_back(solution.block<3,1>(i*3,0));
transformation_t transformation = math::arun_complete(c,ccam);
t = transformation.col(3);
R = transformation.block<3,3>(0,0);
//transform world points into camera frame and compute the error
double error = 0.0;
for(size_t i = 0; i<4; i++)
{
point_t ccam_reprojected = R.transpose() * (c[i] - t);
error +=
1.0 -
(ccam_reprojected.dot(ccam[i])/(ccam[i].norm()*ccam_reprojected.norm()));
}
return error;
}
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);
}
}
void
opengv::extractRelativePose(
const translation_t & position1,
const translation_t & position2,
const rotation_t & rotation1,
const rotation_t & rotation2,
translation_t & relativePosition,
rotation_t & relativeRotation,
bool normalize )
{
relativeRotation = rotation1.transpose() * rotation2;
relativePosition = rotation1.transpose() * (position2 - position1);
if(normalize)
relativePosition = relativePosition / relativePosition.norm();
}
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();
}
}
