int aligner(int argc, char **argv)
{
	// load pcds
	pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cld_ptr_original(new pcl::PointCloud<pcl::PointXYZ>);
	
   pcl::io::loadPCDFile<pcl::PointXYZ> ( "/home/natanaso/Desktop/cld_experiment/cld_7189.3909999999996.pcd", *cld_ptr);
   pcl::io::loadPCDFile<pcl::PointXYZ> ( "/home/natanaso/Desktop/cld_experiment/orig_7189.3909999999996.pcd", *xyz_cld_ptr_original);
   
   
	// remove NaNs
	std::vector<int> indices;
	pcl::removeNaNFromPointCloud(*cld_ptr, *cld_ptr, indices);

	
	pcl::PCDWriter writer;
	pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr_tmp(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr_a(new pcl::PointCloud<pcl::PointXYZ>);
	
	pcl::PassThrough<pcl::PointXYZ> pass_;
	pass_.setFilterFieldName ("y");
	pass_.setFilterLimits (-1, 1);
	pass_.setInputCloud(cld_ptr);
	pass_.filter(*cld_ptr_tmp);
	
	pass_.setFilterFieldName ("z");
	pass_.setFilterLimits (0.55, 2.0);	
	pass_.setInputCloud(cld_ptr_tmp);
	pass_.filter(*cld_ptr_a);
		
	cld_ptr_a = pcd_utils::voxel_grid_subsample( cld_ptr_a, 0.003f );
	
	
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/init_0.pcd", *cld_ptr_a + *xyz_cld_ptr_original );	
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/filt_0.pcd", *cld_ptr_a );
	
	// Get an initial guess for the transform
	pass_.setFilterLimits (0.75, 2.0);
	pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr_filt(new pcl::PointCloud<pcl::PointXYZ>);
	pass_.setInputCloud(cld_ptr_a);
	pass_.filter(*cld_ptr_filt);
	pcl::PointCloud<pcl::PointXYZ>::Ptr xyz_cld_ptr_filt(new pcl::PointCloud<pcl::PointXYZ>);
	pass_.setInputCloud(xyz_cld_ptr_original);
	pass_.filter(*xyz_cld_ptr_filt);

	
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/filt_a.pcd", *cld_ptr_filt );
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/filt_b.pcd", *xyz_cld_ptr_filt );
		
	double fitness_score = 1000;
	Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f fTrans = Eigen::Matrix4f::Identity();
	pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	tmp_ptr = pcd_utils::align_clouds( cld_ptr_filt, xyz_cld_ptr_filt, 0.2, guess, fTrans, fitness_score );
	
	/*
	// XXX: USE PCL17
	pcd17_util::align_clouds17( cld_ptr_filt->getMatrixXfMap(),
										xyz_cld_ptr_filt->getMatrixXfMap(),
										0.5, guess, fTrans, fitness_score );
	pcl::transformPointCloud ( *cld_ptr_filt, *tmp_ptr, fTrans );
	// *** END ***
	*/
	
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/combined_a.pcd", *tmp_ptr + * xyz_cld_ptr_filt);
	
	// Use the guess to allign a larger portion of the cloud
	//	pass_.setFilterLimits (0.55, 2.0);
	//	pass_.setInputCloud(cld_ptr);
	//	pass_.filter(*cld_ptr_filt);
	//	pass_.setInputCloud(xyz_cld_ptr_original);
	//	pass_.filter(*xyz_cld_ptr_filt);
	//	guess = fTrans;
	//	tmp_ptr = pcd_utils::align_clouds( cld_ptr_filt, xyz_cld_ptr_filt, 0.2, guess, fTrans, fitness_score );

	// Use the guess to align the large cloud	
	guess = fTrans;
	tmp_ptr = pcd_utils::align_clouds( cld_ptr_a, xyz_cld_ptr_original, 0.2, guess, fTrans, fitness_score );	


	Eigen::Matrix4f best_trans = fTrans;
	double best_score = fitness_score;
	if( fitness_score > 0.0005 )
	{
		const double arr[] = {0.2, 0.0, -0.2, 0.0, 0.0, 0.2, 0.0, -0.2};
		
		for(int k = 0; k < 4; ++k)
		{
			// generate the offset cloud above the table
			pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr_off(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cld_ptr_filt, *cld_ptr_off);
			for( pcl::PointCloud<pcl::PointXYZ>::iterator it = cld_ptr_off->begin();
				it != cld_ptr_off->end(); ++it)
			{
				(*it).x += arr[2*k];
				(*it).y += arr[2*k+1];
			}
			guess = Eigen::Matrix4f::Identity();
			pcd_utils::align_clouds( cld_ptr_off, xyz_cld_ptr_filt, 0.2, guess, fTrans, fitness_score );	
			
			
			pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr_a_off(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cld_ptr_a, *cld_ptr_a_off);
			for( pcl::PointCloud<pcl::PointXYZ>::iterator it = cld_ptr_a_off->begin();
				it != cld_ptr_a_off->end(); ++it)
			{
				(*it).x += arr[2*k];
				(*it).y += arr[2*k+1];
			}		
			guess = fTrans;
			pcd_utils::align_clouds( cld_ptr_a_off, xyz_cld_ptr_original, 0.2, guess, fTrans, fitness_score );
			
			/*
			if( k == 3)
			{
				pcl::transformPointCloud ( *cld_ptr_a_off, *tmp_ptr, fTrans );
				writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/combined_b.pcd", *tmp_ptr + *xyz_cld_ptr_original );
			}
			*/
			
			if( fitness_score < best_score)
			{
				best_score = fitness_score;
				
				Eigen::Matrix4f T = Eigen::Matrix4f::Identity();				
				T(0,3) = arr[2*k];
				T(1,3) = arr[2*k+1];
				best_trans = fTrans * T;
			}	
		}
	}
				
								
	pcl::PointCloud<pcl::PointXYZ>::Ptr best_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::transformPointCloud ( *cld_ptr_a, *best_ptr, best_trans );
   
   
   cld_ptr = best_ptr;	
	*xyz_cld_ptr_original = *cld_ptr + *xyz_cld_ptr_original;		// add the two clouds

	//writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/filt_c.pcd", *cld_ptr_filt );
	//writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/filt_d.pcd", *xyz_cld_ptr_filt );
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/combined1.pcd", *xyz_cld_ptr_original );
   
   /*
   // First cut the table off
	// first align only the clouds above the table to get a guess for the transformation
	Eigen::Matrix4f guess = Eigen::Matrix4f::Identity();
	Eigen::Matrix4f fTrans = Eigen::Matrix4f::Identity();


	pcl::PointCloud<pcl::PointXYZ>::Ptr cam_cld_ptr_filt(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr head_cld_ptr_filt(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_ptr(new pcl::PointCloud<pcl::PointXYZ>);
	
	pcl::PassThrough<pcl::PointXYZ> pass_;
	pass_.setFilterFieldName ("z");
	pass_.setFilterLimits (-0.35, 2.5);
	pass_.setInputCloud(cam_cld_ptr);
	pass_.filter(*cam_cld_ptr_filt);
	pass_.setInputCloud(head_cld_ptr);
	pass_.filter(*head_cld_ptr_filt);

	
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp1;
	icp1.setMaxCorrespondenceDistance(0.1);
	icp1.setTransformationEpsilon(1e-6);	// transformation convergence epsilon
	icp1.setInputCloud(cam_cld_ptr_filt);
	icp1.setInputTarget(head_cld_ptr_filt);
	icp1.align(*tmp_ptr, guess);
	std::cout << "has converged:" << icp1.hasConverged() << " score: " <<
	icp1.getFitnessScore() << std::endl;
	std::cout << icp1.getFinalTransformation() << std::endl;	
	
	fTrans = icp1.getFinalTransformation();
	
			
	pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
	icp.setMaxCorrespondenceDistance(0.1);
	icp.setTransformationEpsilon(1e-6);	// transformation convergence epsilon
	//icp.setEuclideanFitnessEpsilon(1e-8);
	
	icp.setInputCloud(cam_cld_ptr);
	icp.setInputTarget(head_cld_ptr);
	pcl::PointCloud<pcl::PointXYZ> Final;
	icp.align(Final,fTrans);
	std::cout << "has converged:" << icp.hasConverged() << " score: " <<
	icp.getFitnessScore() << std::endl;
	std::cout << icp.getFinalTransformation() << std::endl;
	
	
	// Save clouds for viewing
	pcl::PCDWriter writer;
	Final += *head_cld_ptr;
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/filt_b.pcd", *head_cld_ptr_filt );
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/combined_a.pcd", *tmp_ptr+*head_cld_ptr_filt );
	writer.writeASCII( "/home/natanaso/Desktop/cld_experiment/combined1.pcd", Final );
   */
	return 0;
}
const unsigned char * tissuestack::imaging::DicomFileWrapper::getData()
{
	// the dicom image
	std::unique_ptr<DicomImage> dcmTmp(new DicomImage(this->_file_name.c_str(), CIF_AcrNemaCompatibility));

	unsigned long data_size = dcmTmp->getOutputDataSize(
		(this->getAllocatedBits() <= 8 || this->isColor()) ? 8 : 16);
	unsigned long image_size = this->getWidth() * this->getHeight();
	if (data_size == 0)
		data_size = image_size * (this->getAllocatedBits() / 2);
	if (image_size == 0)
		image_size = data_size;
	const unsigned long finished_image_size = image_size * 3;

	// we converge on an RGB format 8 bit per channel which is what our raw format is
	std::unique_ptr<unsigned char[]> data_ptr(new unsigned char[finished_image_size]);

	double min = pow(2, this->getAllocatedBits())-1;
	double max = 0;
	dcmTmp->getMinMaxValues(min,max);

	int two_power8 = static_cast<int>(pow(2, 8));
	int two_power_16 = static_cast<int>(pow(2, 16));

	if (this->isColor()) // COLOR
	{
		if (dcmTmp->getOutputData(data_ptr.get(), data_size, 8, 0, 0) <= 0)
			return nullptr;

		return data_ptr.release();
	}

	if (this->getAllocatedBits() <= 8) // 8 BIT
	{
		if (this->containsSignedData()) // SIGNED
		{
			std::unique_ptr<char[]> tmp_ptr(new char[image_size]);
			if (dcmTmp->getOutputData(tmp_ptr.get(), data_size, 8, 0, 0) <= 0)
				return nullptr;

			// convert to unsigned
			min = pow(2, this->getAllocatedBits())-1;
			max = 0;
			for (unsigned long i=0;i<image_size;i++)
			{
				const unsigned long rgbOffset = i * 3;
				const unsigned char val =
					static_cast<unsigned char>(static_cast<int>(tmp_ptr.get()[i]) + (two_power8 / 2));
				data_ptr.get()[rgbOffset] = data_ptr.get()[rgbOffset+1] = data_ptr.get()[rgbOffset+2] = val;

				if (val < min)
					min = val;
				if (val > max)
					max = val;
			}

			// now that we have min and max ... fit to contrast range linearly
			for (unsigned long i=0;i<finished_image_size;i++)
				data_ptr.get()[i] =
					static_cast<unsigned char>(((static_cast<double>(data_ptr.get()[i])-min) * (two_power8-1)) / (max-min));

			// return
			return data_ptr.release();
		}

		// UNSIGNED
		std::unique_ptr<unsigned char[]> tmp_ptr(new unsigned char[image_size]);
		if (dcmTmp->getOutputData(tmp_ptr.get(), data_size, 8, 0, 0) <= 0)
			return nullptr;

		// make RGB data
		for (unsigned long i=0;i<image_size;i++)
		{
			const unsigned long rgbOffset = i * 3;
			data_ptr.get()[rgbOffset] =
			data_ptr.get()[rgbOffset+1] =
			data_ptr.get()[rgbOffset+2] = tmp_ptr.get()[i];
		}

		return data_ptr.release();
	}

	if (this->getAllocatedBits() > 8) // 12/16 BITS
	{
		std::unique_ptr<unsigned short[]> tmp_16bit_ptr(new unsigned short[finished_image_size]);

		if (this->containsSignedData()) // SIGNED
		{
			std::unique_ptr<short[]> tmp_ptr(new short[image_size]);
			if (dcmTmp->getOutputData(tmp_ptr.get(), data_size, 16, 0, 0) <= 0)
				return nullptr;

			// convert to unsigned
			min = pow(2, this->getAllocatedBits())-1;
			max = 0;
			for (unsigned long i=0;i<image_size;i++)
			{
				const unsigned long rgbOffset = i * 3;
				const unsigned short val =
					static_cast<unsigned short>(static_cast<int>(tmp_ptr.get()[i]) + (two_power_16 / 2));
				tmp_16bit_ptr.get()[rgbOffset] = tmp_16bit_ptr.get()[rgbOffset+1] = tmp_16bit_ptr.get()[rgbOffset+2] = val;

				if (val < min)
					min = val;
				if (val > max)
					max = val;
			}

			// now that we have min and max ... fit to contrast range linearly to make it 8 bit
			for (unsigned long i=0;i<finished_image_size;i++)
				data_ptr.get()[i] =
					static_cast<unsigned char>(((static_cast<double>(tmp_16bit_ptr.get()[i])-min) * (two_power8-1)) / (max-min));

			return data_ptr.release();
		}

		// UNSIGNED
		if (dcmTmp->getOutputData(tmp_16bit_ptr.get(), data_size, 16, 0, 0) <= 0)
			return nullptr;

		// turn 16 bit into 8 bit
		for (unsigned long i=0;i<image_size;i++)
		{
			const unsigned long rgbOffset = i * 3;
			data_ptr.get()[rgbOffset] = data_ptr.get()[rgbOffset+1] = data_ptr.get()[rgbOffset+2] =
				static_cast<unsigned char>((static_cast<double>(tmp_16bit_ptr.get()[i]) * (two_power8-1)) / (two_power_16-1));
		}
	}

	return data_ptr.release();
}