void TransformationCheckersImpl<T>::BoundTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
{
	typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
	
	if (parameters.rows() == 4)
	{
		const Quaternion currentRotation = Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3)));
		this->conditionVariables(0) = currentRotation.angularDistance(initialRotation3D);
	}
	else if (parameters.rows() == 3)
	{
		const T currentRotation(acos(parameters(0,0)));
		this->conditionVariables(0) = normalizeAngle(currentRotation - initialRotation2D);
	}
	else
		assert(false);
	const unsigned int nbRows = parameters.rows()-1;
	const Vector currentTranslation = parameters.topRightCorner(nbRows,1);
	this->conditionVariables(1) = (currentTranslation - initialTranslation).norm();
	if (this->conditionVariables(0) > this->limits(0) || this->conditionVariables(1) > this->limits(1))
	{
		ostringstream oss;
		oss << "limit out of bounds: ";
		oss << "rot: " << this->conditionVariables(0) << "/" << this->limits(0) << " ";
		oss << "tr: " << this->conditionVariables(1) << "/" << this->limits(1);
		throw ConvergenceError(oss.str());
	}
}

void TransformationCheckersImpl<T>::DifferentialTransformationChecker::check(const TransformationParameters& parameters, bool& iterate)
{
	typedef typename PointMatcher<T>::ConvergenceError ConvergenceError;
	
	rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
	const unsigned int nbRows = parameters.rows()-1;
	translations.push_back(parameters.topRightCorner(nbRows,1));
	
	this->conditionVariables.setZero(2);
	if(rotations.size() > smoothLength)
	{
		for(size_t i = rotations.size()-1; i >= rotations.size()-smoothLength; i--)
		{
			//Compute the mean derivative
			this->conditionVariables(0) += anyabs(rotations[i].angularDistance(rotations[i-1]));
			this->conditionVariables(1) += anyabs((translations[i] - translations[i-1]).norm());
		}

		this->conditionVariables /= smoothLength;

		if(this->conditionVariables(0) < this->limits(0) && this->conditionVariables(1) < this->limits(1))
			iterate = false;
	}
	
	//std::cout << "Abs Rotation: " << this->conditionVariables(0) << " / " << this->limits(0) << std::endl;
	//std::cout << "Abs Translation: " << this->conditionVariables(1) << " / " << this->limits(1) << std::endl;
	
	if (boost::math::isnan(this->conditionVariables(0)))
		throw ConvergenceError("abs rotation norm not a number");
	if (boost::math::isnan(this->conditionVariables(1)))
		throw ConvergenceError("abs translation norm not a number");
}

bool TransformationsImpl<T>::RigidTransformation::checkParameters(const TransformationParameters& parameters) const
{
	//FIXME: FP - should we put that as function argument?
	const T epsilon = 0.001;

	const TransformationParameters R(parameters.topLeftCorner(parameters.rows()-1, parameters.cols()-1));
	
	if(anyabs(1 - R.determinant()) > epsilon)
		return false;
	else
		return true;
}

typename PointMatcher<T>::DataPoints TransformationsImpl<T>::PureTranslation::compute(const DataPoints& input,
		const TransformationParameters& parameters) const {
	assert(input.features.rows() == parameters.rows());
	assert(parameters.rows() == parameters.cols());

	if(this->checkParameters(parameters) == false)
		throw PointMatcherSupport::TransformationError("PureTranslation: Error, left part  not identity.");

	DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.features.cols());

	// Apply the transformation to features
	transformedCloud.features = parameters * input.features;

	return transformedCloud;
}

bool TransformationsImpl<T>::PureTranslation::checkParameters(
		const TransformationParameters& parameters) const {
	const int rows = parameters.rows();
	const int cols = parameters.cols();

	// make a copy of parameters to perform the check
	TransformationParameters parameters_(parameters);

	// set the translation components of the transformation matrix to 0
	parameters_.block(0,cols-1,rows-1,1).setZero();

	// If we have the identity matrix, than this is indeed a pure translation
	if (parameters_.isApprox(TransformationParameters::Identity(rows,cols)))
		return true;
	else
		return false;
}

typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::PureTranslation::correctParameters(
		const TransformationParameters& parameters) const {
	const int rows = parameters.rows();
	const int cols = parameters.cols();

	// make a copy of the parameters to perform corrections on
	TransformationParameters correctedParameters(parameters);

	// set the top left block to the identity matrix
	correctedParameters.block(0,0,rows-1,cols-1).setIdentity();

	// fix the bottom row
	correctedParameters.block(rows-1,0,1,cols-1).setZero();
	correctedParameters(rows-1,cols-1) = 1;

	return correctedParameters;
}

typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::RigidTransformation::correctParameters(const TransformationParameters& parameters) const
{
	TransformationParameters ortho = parameters;
	if(ortho.cols() == 4)
	{
		const Eigen::Matrix<T, 3, 1> col0 = parameters.block(0, 0, 3, 1).normalized();
		const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized();
		const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized();


		ortho.block(0, 0, 3, 1) = col1.cross(col2);
		ortho.block(0, 1, 3, 1) = col2.cross(col0);
		ortho.block(0, 2, 3, 1) = col2;
	}
	else if(ortho.cols() == 3)
	{
		// R = [ a b]
		//     [-b a]
		
		// mean of a and b
		T a = (parameters(0,0) + parameters(1,1))/2; 	
		T b = (-parameters(1,0) + parameters(0,1))/2;
		T sum = sqrt(pow(a,2) + pow(b,2));

		a = a/sum;
		b = b/sum;

		ortho(0,0) =  a; ortho(0,1) = b;
		ortho(1,0) = -b; ortho(1,1) = a;
	}


	return ortho;
}

typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::TransformationWithScale::correctParameters(
		const TransformationParameters& parameters) const {
	std::cout << "Correcting Paramsa. .. .. . . " << std::endl;

	const int rows = parameters.rows();
	const int cols = parameters.cols();

	TransformationParameters correctedParameters(parameters);
	return correctedParameters;

	// make a copy of the parameters to perform corrections on
	// // set the top left block to the identity matrix
	// correctedParameters.block(0,0,rows-1,cols-1).setIdentity();

	// // fix the bottom row
	// correctedParameters.block(rows-1,0,1,cols-1).setZero();
	// correctedParameters(rows-1,cols-1) = 1;

	// return correctedParameters;
}

void TransformationCheckersImpl<T>::DifferentialTransformationChecker::init(const TransformationParameters& parameters, bool& iterate)
{
	this->conditionVariables.setZero(2);
	
	rotations.clear();
	translations.clear();
	
	if (parameters.rows() == 4)
	{
		rotations.push_back(Quaternion(Eigen::Matrix<T,3,3>(parameters.topLeftCorner(3,3))));
	}
	else
	{
		// Handle the 2D case
		Eigen::Matrix<T,3,3> m(Matrix::Identity(3,3));
		m.topLeftCorner(2,2) = parameters.topLeftCorner(2,2);
		rotations.push_back(Quaternion(m));
	}
	
	const unsigned int nbRows = parameters.rows()-1;
	translations.push_back(parameters.topRightCorner(nbRows,1));
}

typename PointMatcher<T>::DataPoints TransformationsImpl<T>::RigidTransformation::compute(
	const DataPoints& input,
	const TransformationParameters& parameters) const
{
	typedef typename PointMatcher<T>::Matrix Matrix;
	
	assert(input.features.rows() == parameters.rows());
	assert(parameters.rows() == parameters.cols());

	const unsigned int nbRows = parameters.rows()-1;
	const unsigned int nbCols = parameters.cols()-1;

	const TransformationParameters R(parameters.topLeftCorner(nbRows, nbCols));

	if(this->checkParameters(parameters) == false)	
		throw TransformationError("RigidTransformation: Error, rotation matrix is not orthogonal.");	
	
	DataPoints transformedCloud(input.featureLabels, input.descriptorLabels, input.timeLabels, input.features.cols());
	
	// Apply the transformation to features
	transformedCloud.features = parameters * input.features;
	
	// Apply the transformation to descriptors
	int row(0);
	const int descCols(input.descriptors.cols());
	for (size_t i = 0; i < input.descriptorLabels.size(); ++i)
	{
		const int span(input.descriptorLabels[i].span);
		const std::string& name(input.descriptorLabels[i].text);
		const BOOST_AUTO(inputDesc, input.descriptors.block(row, 0, span, descCols));
		BOOST_AUTO(outputDesc, transformedCloud.descriptors.block(row, 0, span, descCols));
		if (name == "normals" || name == "observationDirections")
			outputDesc = R * inputDesc;
		else
			outputDesc = inputDesc;
		row += span;
	}
	
	return transformedCloud;
}

typename PointMatcher<T>::TransformationParameters PointMatcher<T>::ICP::computeWithTransformedReference(
	const DataPoints& readingIn, 
	const DataPoints& reference, 
	const TransformationParameters& T_refIn_refMean,
	const TransformationParameters& T_refIn_dataIn)
{
	timer t; // Print how long take the algo
	
	// Apply readings filters
	// reading is express in frame <dataIn>
	DataPoints reading(readingIn);
	//const int nbPtsReading = reading.features.cols();
	this->readingDataPointsFilters.init();
	this->readingDataPointsFilters.apply(reading);
	readingFiltered = reading;
	
	// Reajust reading position: 
	// from here reading is express in frame <refMean>
	TransformationParameters 
		T_refMean_dataIn = T_refIn_refMean.inverse() * T_refIn_dataIn;
	this->transformations.apply(reading, T_refMean_dataIn);
	
	// Prepare reading filters used in the loop 
	this->readingStepDataPointsFilters.init();
	
	// Since reading and reference are express in <refMean>
	// the frame <refMean> is equivalent to the frame <iter(0)>
	const int dim(reference.features.rows());
	TransformationParameters T_iter = Matrix::Identity(dim, dim);
	
	bool iterate(true);
	this->transformationCheckers.init(T_iter, iterate);

	size_t iterationCount(0);
	
	// statistics on last step
	this->inspector->addStat("ReadingPreprocessingDuration", t.elapsed());
	this->inspector->addStat("ReadingInPointCount", readingIn.features.cols());
	this->inspector->addStat("ReadingPointCount", reading.features.cols());
	LOG_INFO_STREAM("PointMatcher::icp - reading pre-processing took " << t.elapsed() << " [s]");
	this->prefilteredReadingPtsCount = reading.features.cols();
	t.restart();
	
	// iterations
	while (iterate)
	{
		DataPoints stepReading(reading);
		
		//-----------------------------
		// Apply step filter
		this->readingStepDataPointsFilters.apply(stepReading);
		
		//-----------------------------
		// Transform Readings
		this->transformations.apply(stepReading, T_iter);
		
		//-----------------------------
		// Match to closest point in Reference
		const Matches matches(
			this->matcher->findClosests(stepReading)
		);
		
		//-----------------------------
		// Detect outliers
		const OutlierWeights outlierWeights(
			this->outlierFilters.compute(stepReading, reference, matches)
		);
		
		assert(outlierWeights.rows() == matches.ids.rows());
		assert(outlierWeights.cols() == matches.ids.cols());
		
		//cout << "outlierWeights: " << outlierWeights << "\n";
	
		
		//-----------------------------
		// Dump
		this->inspector->dumpIteration(
			iterationCount, T_iter, reference, stepReading, matches, outlierWeights, this->transformationCheckers
		);
		
		//-----------------------------
		// Error minimization
		// equivalent to: 
		//   T_iter(i+1)_iter(0) = T_iter(i+1)_iter(i) * T_iter(i)_iter(0)
		T_iter = this->errorMinimizer->compute(
			stepReading, reference, outlierWeights, matches) * T_iter;
		
		// Old version
		//T_iter = T_iter * this->errorMinimizer->compute(
		//	stepReading, reference, outlierWeights, matches);
		
		// in test
		
		this->transformationCheckers.check(T_iter, iterate);
	
		++iterationCount;
	}
	
	this->inspector->addStat("IterationsCount", iterationCount);
	this->inspector->addStat("PointCountTouched", this->matcher->getVisitCount());
//.........这里部分代码省略.........

FinalRepresentation
IntermediateRepresentator::transform(const TransformationParameters& parameters,
                                     const CompileErrorList& parsingErrors,
                                     MemoryAccess& memoryAccess) {
  auto errors = parsingErrors;
  if (_currentOutput) {
    errors.pushError(_currentOutput->positionInterval(),
                     "Macro not closed. Missing a macro end directive?");
  }

  PrecompileImmutableArguments precompileArguments(parameters.architecture(),
                                                   parameters.generator());

  SymbolGraph graph;
  MacroDirectiveTable macroTable;
  for (const auto& command : _commandList) {
    command->precompile(precompileArguments, errors, graph, macroTable);
  }

  IntermediateMacroInstruction::replaceWithMacros(
      _commandList.begin(), _commandList.end(), macroTable, errors);

  auto macroList = generateMacroInformation();
  auto preliminaryEvaluation = graph.evaluate();

  if (!evaluateGraph(preliminaryEvaluation, errors)) {
    return FinalRepresentation({}, errors, macroList);
  }

  SymbolReplacer preliminaryReplacer(preliminaryEvaluation);

  MemoryAllocator allocator(parameters.allocator());
  SectionTracker tracker;

  auto allowedSize = memoryAccess.getMemorySize().get();
  IntermediateOperationPointer firstMemoryExceedingOperation(nullptr);

  AllocateMemoryImmutableArguments allocateMemoryArguments(precompileArguments,
                                                           preliminaryReplacer);

  for (const auto& command : _commandList) {
    command->allocateMemory(
        allocateMemoryArguments, errors, allocator, tracker);
    if (allocator.estimateSize() > allowedSize &&
        !firstMemoryExceedingOperation) {
      firstMemoryExceedingOperation = command;
    }
  }

  auto allocatedSize = allocator.calculatePositions();

  EnhanceSymbolTableImmutableArguments symbolTableArguments(
      allocateMemoryArguments, allocator);
  for (const auto& command : _commandList) {
    command->enhanceSymbolTable(symbolTableArguments, errors, graph);
  }

  auto graphEvaluation = graph.evaluate();
  auto graphValid = evaluateGraph(graphEvaluation, errors);
  auto memoryValid = checkMemorySize(
      allocatedSize, allowedSize, firstMemoryExceedingOperation, errors);
  if (!(graphValid && memoryValid)) {
    return FinalRepresentation({}, errors, macroList);
  }

  SymbolReplacer replacer(graphEvaluation);
  ExecuteImmutableArguments executeArguments(symbolTableArguments, replacer);
  FinalCommandVector commandOutput;
  for (const auto& command : _commandList) {
    command->execute(executeArguments, errors, commandOutput, memoryAccess);
  }

  return FinalRepresentation(commandOutput, errors, macroList);
}