592 lines
20 KiB
C++
592 lines
20 KiB
C++
/******************************************************************************
|
|
* Author: Laurent Kneip *
|
|
* Contact: kneip.laurent@gmail.com *
|
|
* License: Copyright (c) 2013 Laurent Kneip, ANU. All rights reserved. *
|
|
* *
|
|
* Redistribution and use in source and binary forms, with or without *
|
|
* modification, are permitted provided that the following conditions *
|
|
* are met: *
|
|
* * Redistributions of source code must retain the above copyright *
|
|
* notice, this list of conditions and the following disclaimer. *
|
|
* * Redistributions in binary form must reproduce the above copyright *
|
|
* notice, this list of conditions and the following disclaimer in the *
|
|
* documentation and/or other materials provided with the distribution. *
|
|
* * Neither the name of ANU nor the names of its contributors may be *
|
|
* used to endorse or promote products derived from this software without *
|
|
* specific prior written permission. *
|
|
* *
|
|
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"*
|
|
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE *
|
|
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE *
|
|
* ARE DISCLAIMED. IN NO EVENT SHALL ANU OR THE CONTRIBUTORS BE LIABLE *
|
|
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL *
|
|
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR *
|
|
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER *
|
|
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT *
|
|
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY *
|
|
* OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF *
|
|
* SUCH DAMAGE. *
|
|
******************************************************************************/
|
|
|
|
#include "experiment_helpers.hpp"
|
|
#include "random_generators.hpp"
|
|
#include <opengv/math/cayley.hpp>
|
|
#include <iostream>
|
|
#include <iomanip>
|
|
|
|
|
|
void
|
|
opengv::generateCentralCameraSystem(
|
|
translations_t & camOffsets,
|
|
rotations_t & camRotations )
|
|
{
|
|
//create a fake camera system for a central camera
|
|
camOffsets.push_back(Eigen::Vector3d::Zero());
|
|
camRotations.push_back(Eigen::Matrix3d::Identity());
|
|
}
|
|
|
|
void
|
|
opengv::generateRandomCameraSystem(
|
|
int numberCameras,
|
|
translations_t & camOffsets,
|
|
rotations_t & camRotations )
|
|
{
|
|
double offset = 0.5; //this is the distance from the viewpoint origin
|
|
|
|
for( int i = 0; i < numberCameras; i++ )
|
|
{
|
|
translation_t camOffset = generateRandomDirectionTranslation(offset);
|
|
rotation_t camRotation = generateRandomRotation();
|
|
camOffsets.push_back(camOffset);
|
|
camRotations.push_back(camRotation);
|
|
}
|
|
}
|
|
|
|
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::printExperimentCharacteristics(
|
|
const translation_t & position,
|
|
const rotation_t & rotation,
|
|
double noise,
|
|
double outlierFraction )
|
|
{
|
|
std::cout << "the random position is:" << std::endl;
|
|
std::cout << position << std::endl << std::endl;
|
|
std::cout << "the random rotation is:" << std::endl;
|
|
std::cout << rotation << std::endl << std::endl;
|
|
std::cout << "the noise in the data is:" << std::endl;
|
|
std::cout << noise << std::endl;
|
|
std::cout << "the outlier fraction is:" << std::endl;
|
|
std::cout << outlierFraction << std::endl;
|
|
}
|
|
|
|
void
|
|
opengv::printBearingVectorArraysMatlab(
|
|
const bearingVectors_t & bearingVectors1,
|
|
const bearingVectors_t & bearingVectors2 )
|
|
{
|
|
size_t numberBearingVectors = bearingVectors1.size();
|
|
|
|
//temp: print the vectors in matlab format (for debugging purposes)
|
|
size_t precision = 10;
|
|
std::cout << "ov1 = [";
|
|
for( size_t i = 0; i < numberBearingVectors; i++ )
|
|
std::cout << std::setprecision(precision) << bearingVectors1[i](0,0) << " ";
|
|
std::cout << ";" << std::endl;
|
|
for( size_t i = 0; i < 8; i++ )
|
|
std::cout << std::setprecision(precision) << bearingVectors1[i](1,0) << " ";
|
|
std::cout << ";" << std::endl;
|
|
for( size_t i = 0; i < 8; i++ )
|
|
std::cout << std::setprecision(precision) << bearingVectors1[i](2,0) << " ";
|
|
std::cout << "];" << std::endl;
|
|
std::cout << "ov2 = [";
|
|
for( size_t i = 0; i < 8; i++ )
|
|
std::cout << std::setprecision(precision) << bearingVectors2[i](0,0) << " ";
|
|
std::cout << ";" << std::endl;
|
|
for( size_t i = 0; i < 8; i++ )
|
|
std::cout << std::setprecision(precision) << bearingVectors2[i](1,0) << " ";
|
|
std::cout << ";" << std::endl;
|
|
for( size_t i = 0; i < 8; i++ )
|
|
std::cout << std::setprecision(precision) << bearingVectors2[i](2,0) << " ";
|
|
std::cout << "];" << std::endl;
|
|
}
|
|
|
|
void
|
|
opengv::printEssentialMatrix(
|
|
const translation_t & position,
|
|
const rotation_t & rotation)
|
|
{
|
|
//E transforms vectors from vp 2 to 1: x_1^T * E * x_2 = 0
|
|
//and E = (t)_skew*R
|
|
Eigen::Matrix3d t_skew = Eigen::Matrix3d::Zero();
|
|
t_skew(0,1) = -position[2];
|
|
t_skew(0,2) = position[1];
|
|
t_skew(1,0) = position[2];
|
|
t_skew(1,2) = -position[0];
|
|
t_skew(2,0) = -position[1];
|
|
t_skew(2,1) = position[0];
|
|
|
|
essential_t E = t_skew * rotation;
|
|
double matrixNorm = 0.0;
|
|
for(size_t r = 0; r < 3; r++)
|
|
{
|
|
for(size_t c = 0; c < 3; c++)
|
|
matrixNorm += pow(E(r,c),2);
|
|
}
|
|
matrixNorm = sqrt(matrixNorm);
|
|
for(size_t r = 0; r < 3; r++)
|
|
{
|
|
for(size_t c = 0; c < 3; c++)
|
|
E(r,c) = E(r,c) / matrixNorm;
|
|
}
|
|
|
|
std::cout << "the random essential matrix is:" << std::endl;
|
|
std::cout << E << std::endl;
|
|
}
|
|
|
|
void
|
|
opengv::getPerturbedPose(
|
|
const translation_t & position,
|
|
const rotation_t & rotation,
|
|
translation_t & perturbedPosition,
|
|
rotation_t & perturbedRotation,
|
|
double amplitude )
|
|
{
|
|
perturbedPosition = position;
|
|
cayley_t cayley = math::rot2cayley(rotation);
|
|
for( size_t i = 0; i < 3; i++ )
|
|
{
|
|
perturbedPosition[i] =
|
|
perturbedPosition[i] +
|
|
(((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*amplitude;
|
|
cayley[i] =
|
|
cayley[i] + (((double) rand())/ ((double) RAND_MAX)-0.5)*2.0*amplitude;
|
|
}
|
|
perturbedRotation = math::cayley2rot(cayley);
|
|
}
|
|
|
|
std::vector<int>
|
|
opengv::getNindices( int n )
|
|
{
|
|
std::vector<int> indices;
|
|
for(int i = 0; i < n; i++)
|
|
indices.push_back(i);
|
|
return indices;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
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();
|
|
}
|
|
}
|
|
|
|
void
|
|
opengv::generateMulti2D3DCorrespondences(
|
|
const translation_t & position,
|
|
const rotation_t & rotation,
|
|
const translations_t & camOffsets,
|
|
const rotations_t & camRotations,
|
|
size_t pointsPerCam,
|
|
double noise,
|
|
double outlierFraction,
|
|
std::vector<std::shared_ptr<points_t> > & multiPoints,
|
|
std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors,
|
|
std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt )
|
|
{
|
|
//initialize point-cloud
|
|
double minDepth = 4;
|
|
double maxDepth = 8;
|
|
|
|
for( size_t cam = 0; cam < camOffsets.size(); cam++ )
|
|
{
|
|
std::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
|
|
std::shared_ptr<points_t> points(new points_t());
|
|
std::shared_ptr<bearingVectors_t> bearingVectors(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++ )
|
|
{
|
|
points->push_back(gt[cam]->col(i));
|
|
|
|
//project the point into the viewpoint frame
|
|
point_t bodyPoint = rotation.transpose()*(gt[cam]->col(i) - position);
|
|
|
|
//project that point into the camera
|
|
bearingVectors->push_back( camRotation.transpose()*(bodyPoint - camOffset) );
|
|
|
|
//normalize the vector
|
|
(*bearingVectors)[i] = (*bearingVectors)[i] / (*bearingVectors)[i].norm();
|
|
|
|
//add noise
|
|
if( noise > 0.0 )
|
|
(*bearingVectors)[i] = addNoise(noise,(*bearingVectors)[i]);
|
|
}
|
|
|
|
//push back the stuff for this camera
|
|
multiPoints.push_back(points);
|
|
multiBearingVectors.push_back(bearingVectors);
|
|
}
|
|
|
|
//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 = rotation.transpose()*(p - position);
|
|
|
|
//use as measurement (outlier)
|
|
(*(multiBearingVectors[cam].get()))[i] =
|
|
camRotation.transpose()*(bodyPoint - camOffset);
|
|
|
|
//normalize
|
|
(*(multiBearingVectors[cam].get()))[i] =
|
|
(*(multiBearingVectors[cam].get()))[i] / (*(multiBearingVectors[cam].get()))[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::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::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<std::shared_ptr<bearingVectors_t> > & multiBearingVectors1,
|
|
std::vector<std::shared_ptr<bearingVectors_t> > & multiBearingVectors2,
|
|
std::vector<std::shared_ptr<Eigen::MatrixXd> > & gt )
|
|
{
|
|
//initialize point-cloud
|
|
double minDepth = 4;
|
|
double maxDepth = 8;
|
|
|
|
for( size_t cam = 0; cam < camOffsets.size(); cam++ )
|
|
{
|
|
std::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
|
|
std::shared_ptr<bearingVectors_t> bearingVectors1(new bearingVectors_t());
|
|
std::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();
|
|
}
|
|
}
|
|
}
|