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;
}