/* +---------------------------------------------------------------------------+
   |                     Mobile Robot Programming Toolkit (MRPT)               |
   |                          http://www.mrpt.org/                             |
   |                                                                           |
   | Copyright (c) 2005-2015, Individual contributors, see AUTHORS file        |
   | See: http://www.mrpt.org/Authors - All rights reserved.                   |
   | Released under BSD License. See details in http://www.mrpt.org/License    |
   +---------------------------------------------------------------------------+ */

#include "slam-precomp.h"   // Precompiled headers

#include <mrpt/slam/observations_overlap.h>
#include <mrpt/maps/CPointsMap.h>

using namespace mrpt::slam;
using namespace mrpt::maps;
using namespace mrpt::obs;
using namespace mrpt::utils;
using namespace mrpt::poses;
using namespace std;

/** Estimates the "overlap" or "matching ratio" of two observations (range [0,1]), possibly taking into account their relative positions.
  *  \note This is used in mrpt::slam::CIncrementalMapPartitioner
  */
double mrpt::slam::observationsOverlap(
	const mrpt::obs::CObservation* o1,
	const mrpt::obs::CObservation* o2,
	const mrpt::poses::CPose3D *pose_o2_wrt_o1 )
{
	if (IS_CLASS(o1,CObservation2DRangeScan) && IS_CLASS(o2,CObservation2DRangeScan))
	{
		const CObservation2DRangeScan *this_obs = static_cast<const CObservation2DRangeScan *>(o1);
		const CObservation2DRangeScan *obs      = static_cast<const CObservation2DRangeScan *>(o2);

		const CPointsMap *map1 = this_obs->buildAuxPointsMap<mrpt::maps::CPointsMap>();
		const CPointsMap *map2 = obs->buildAuxPointsMap<mrpt::maps::CPointsMap>();

		// if PDF is available, get "mean" value as an estimation:
		CPose3D	  otherObsPose;
		if (pose_o2_wrt_o1)
			otherObsPose = *pose_o2_wrt_o1;

		mrpt::utils::TMatchingPairList	correspondences;
		mrpt::maps::TMatchingParams matchParams;
		mrpt::maps::TMatchingExtraResults matchExtraResults;

		matchParams.maxDistForCorrespondence = 0.04f;
		matchParams.maxAngularDistForCorrespondence = 0;
		map1->determineMatching3D(
			map2,				// The other map
			otherObsPose,		// The other map pose
			correspondences,
			matchParams, matchExtraResults);

		return matchExtraResults.correspondencesRatio;
	}
	else
	{
		// No idea...
		return 0;
	}

}

/** Estimates the "overlap" or "matching ratio" of two set of observations (range [0,1]), possibly taking into account their relative positions.
  *   This method computes the average between each of the observations in each SF.
  *  \note This is used in mrpt::slam::CIncrementalMapPartitioner
  */
double mrpt::slam::observationsOverlap(
	const mrpt::obs::CSensoryFrame &sf1,
	const mrpt::obs::CSensoryFrame &sf2,
	const mrpt::poses::CPose3D *pose_sf2_wrt_sf1 )
{
	MRPT_UNUSED_PARAM(pose_sf2_wrt_sf1);
	// Return the average value:
	size_t N=0;
	double accum = 0;
	for (CSensoryFrame::const_iterator i1=sf1.begin();i1!=sf1.end();++i1)
	{
		for (CSensoryFrame::const_iterator i2=sf2.begin();i2!=sf2.end();++i2)
		{
			accum += observationsOverlap(*i1,*i2);
			N++;
		}
	}
	return  N ? (accum/N) : 0;
}


