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