void
OrganizedMultiPlaneExtractor<PointT>::compute()
{
CHECK ( cloud_ && cloud_->isOrganized() ) << "Input cloud is not organized!";
CHECK ( normal_cloud_ && (normal_cloud_->points.size() == cloud_->points.size() ));
pcl::OrganizedMultiPlaneSegmentation<PointT, pcl::Normal, pcl::Label> mps;
mps.setMinInliers ( param_.min_num_plane_inliers_ );
mps.setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f );
mps.setDistanceThreshold ( param_.distance_threshold_);
mps.setInputNormals ( normal_cloud_ );
mps.setInputCloud ( cloud_ );
std::vector < pcl::PlanarRegion<PointT>, Eigen::aligned_allocator<pcl::PlanarRegion<PointT> > > regions;
typename pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label>::Ptr ref_comp (
new pcl::PlaneRefinementComparator<PointT, pcl::Normal, pcl::Label> ());
ref_comp->setDistanceThreshold (param_.distance_threshold_, false );
ref_comp->setAngularThreshold ( param_.angular_threshold_deg_ * M_PI/180.f );
mps.setRefinementComparator ( ref_comp );
mps.segmentAndRefine ( regions );
all_planes_.clear();
all_planes_.reserve (regions.size ());
for (const pcl::PlanarRegion<PointT> ® : regions )
{
Eigen::Vector4f plane = reg.getCoefficients();
// flip plane normal towards viewpoint
Eigen::Vector3f z = Eigen::Vector3f::UnitZ();
if( z.dot(plane.head(3)) > 0 )
plane = -plane;
all_planes_.push_back( plane );
}
}
bool PhotoCamera::initializeFromMeshLab(QDomElement &element)
{
QStringList attTemp;
//Get Translation
attTemp = element.attribute("TranslationVector").split(" ");
Eigen::Vector4f translation;
for(int i = 0; i < attTemp.count(); i++){
translation(i) = attTemp[i].toFloat();
}
translationMatrix.translation() = translation.head(3);
//translationMatrix.col(3) = translation;
//Get Center;
attTemp = element.attribute("CenterPx").split(" ");
principalPoint << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
//Get RotationMatrix;
attTemp = element.attribute("RotationMatrix").split(" ");
for(int i = 0; i < 4; i++){
for(int j = 0; j < 4; j++){
rotationMatrix(i, j) = attTemp[i*4 + j].toFloat();
}
}
//Get Viewport;
attTemp = element.attribute("ViewportPx").split(" ");
viewport << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
//Lens Distortion;
attTemp = element.attribute("LensDistortion").split(" ");
distortion << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
//std::cout << distortion.transpose() << std::endl;
//PixelSize;
attTemp = element.attribute("PixelSizeMm").split(" ");
pixelSize << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
oneOverDx = 1/pixelSize(0);
oneOverDy = 1/pixelSize(1);
//std::cout << pixelSize.transpose() << std::endl;
//Focal
focalLength = element.attribute("FocalMm").toFloat();
buildExtrinsic();
buildIntrinsic();
buildProjection();
// cameraCenter = center(intrinsicMatrix.topLeftCorner(3,4)*extrinsicMatrix.matrix());
cameraCenter = center(intrinsicMatrix.matrix().topLeftCorner(3,4)*extrinsicMatrix.matrix());
return true;
}
Eigen::Vector3f unproject(const Eigen::Vector3f &win,
const Eigen::Matrix4f &model,
const Eigen::Matrix4f &proj,
const Vector2i &viewportSize) {
Eigen::Matrix4f Inverse = (proj * model).inverse();
Eigen::Vector4f tmp;
tmp << win, 1;
tmp(0) = tmp(0) / viewportSize.x();
tmp(1) = tmp(1) / viewportSize.y();
tmp = tmp.array() * 2.0f - 1.0f;
Eigen::Vector4f obj = Inverse * tmp;
obj /= obj(3);
return obj.head(3);
}
Eigen::Vector3f project(const Eigen::Vector3f &obj,
const Eigen::Matrix4f &model,
const Eigen::Matrix4f &proj,
const Vector2i &viewportSize) {
Eigen::Vector4f tmp;
tmp << obj, 1;
tmp = model * tmp;
tmp = proj * tmp;
tmp = tmp.array() / tmp(3);
tmp = tmp.array() * 0.5f + 0.5f;
tmp(0) = tmp(0) * viewportSize.x();
tmp(1) = tmp(1) * viewportSize.y();
return tmp.head(3);
}
Eigen::Vector3f igl::unproject(
const Eigen::Vector3f& win,
const Eigen::Matrix4f& model,
const Eigen::Matrix4f& proj,
const Eigen::Vector4f& viewport)
{
Eigen::Matrix4f Inverse = (proj * model).inverse();
Eigen::Vector4f tmp;
tmp << win, 1;
tmp(0) = (tmp(0) - viewport(0)) / viewport(2);
tmp(1) = (tmp(1) - viewport(1)) / viewport(3);
tmp = tmp.array() * 2.0f - 1.0f;
Eigen::Vector4f obj = Inverse * tmp;
obj /= obj(3);
return obj.head(3);
}
//TODO: move to semantics
void
PlaneExtraction::findClosestTable(std::vector<pcl::PointCloud<Point>, Eigen::aligned_allocator<pcl::PointCloud<Point> > >& v_cloud_hull,
std::vector<pcl::ModelCoefficients>& v_coefficients_plane,
Eigen::Vector3f& robot_pose,
unsigned int& idx)
{
std::vector<unsigned int> table_candidates;
for(unsigned int i=0; i<v_cloud_hull.size(); i++)
{
//if(fabs(v_coefficients_plane[i].values[3])>0.5 && fabs(v_coefficients_plane[i].values[3])<1.2)
table_candidates.push_back(i);
}
if(table_candidates.size()>0)
{
for(unsigned int i=0; i<table_candidates.size(); i++)
{
double d_min = 1000;
double d = d_min;
Eigen::Vector4f centroid;
pcl::compute3DCentroid(v_cloud_hull[i], centroid);
//for(unsigned int j=0; j<v_cloud_hull[i].size(); j++)
//{
// Eigen::Vector3f p = v_cloud_hull[i].points[j].getVector3fMap();
// d += fabs((p-robot_pose).norm());
//}
//d /= v_cloud_hull[i].size();
Eigen::Vector3f centroid3 = centroid.head(3);
d = fabs((centroid3-robot_pose).norm());
ROS_INFO("d: %f", d);
if(d<d_min)
{
d_min = d;
idx = table_candidates[i];
}
}
}
}
int main(int argc, char **argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
if(argc<3){
std::cout<<"This node is intend to use offline"<<std::endl<<"provide a logfile (produced with the dumpernode) and the output calibration file you want"<<std::endl;
std::cout<<"example:"<<std::endl<<"offlineCalibration <input log filename> <output calibration filename>"<<std::endl;
return 0;
}
float c0,c1,c2,c3=0;
c0=1.0f;
if(argc==7){
c3=atof(argv[3]);
c2=atof(argv[4]);
c1=atof(argv[5]);
c0=atof(argv[6]);
std::cout<<"polynomial coeffs: d^3*"<<c3<<" + d^2*"<<c2<<" + d*"<<c1<<" + "<<c0<<std::endl;
}
std::fstream log;
log.open(argv[1]);
string topic;
int height;
int width;
log >> topic;
log >> height;
log >> width;
Matrix3f k;
log >> k(0,0);
log >> k(0,1);
log >> k(0,2);
log >> k(1,0);
log >> k(1,1);
log >> k(1,2);
log >> k(2,0);
log >> k(2,1);
log >> k(2,2);
string filename;
//calibrationMatrix c(480,640,8000,4,8);
int rows =480;
int cols = 640;
calibrationMatrix c(rows,cols,8000,4,16);
//calibrationMatrix c(rows,cols,8000,4,64);
while(!log.eof()){
log>>filename;
std::cout<< "opening "<<filename<<"\r"<<std::flush;
cv::Mat data = cv::imread(filename,cv::IMREAD_ANYDEPTH);
pointCloud p(data,k);
//VOXELIZER
pcl::PointCloud<pcl::PointXYZ>* pcl;
pcl=p.pclCloud();
calibration::voxelize(pcl,pcl,20);
pointCloud p2(pcl);
//CENTER SQUARE CLOUD
pcl::PointCloud<pcl::PointXYZ>* square= new pcl::PointCloud<pcl::PointXYZ>();
calibration::computeCenterSquareCloud(data,k,square,cols/10,rows/10,c0,c1,c2,c3); //100 100
pointCloud p3(square);
//CENTER PLANE
Eigen::Vector4f centerModel;
calibration::computerCenterPlane(square,centerModel,40);
std::cout.flush();
planeCloud centerPlaneCloud;
centerPlaneCloud.model=centerModel;
Eigen::Vector4f center;
pcl::compute3DCentroid(*square,center);
//std::cout<<"CENTROID DISTANCE: "<<center[2]<<" ( "<<center.transpose()<<" )"<<std::endl;
centerPlaneCloud.com=center.head(3);
//NORMALS
pcl::PointCloud<pcl::Normal>* normals= new pcl::PointCloud<pcl::Normal>();
calibration::computeNormals(p2.pclCloud(),normals,100);
//REJECTION
Eigen::Vector3f ref;
std::vector<bool> valid;
ref<<centerModel[0],centerModel[1],centerModel[2];
pcl::PointCloud<pcl::PointXYZ> outFromNormalRejection;
pcl::PointCloud<pcl::PointXYZ>* tmpCloud =p2.pclCloud();
calibration::pointrejection(&ref,0.7f,tmpCloud,normals,&outFromNormalRejection,&valid);
//pointCloud outFromNormalRejectionCloud(&outFromNormalRejection);
//ERROR PER POINT
pcl::PointCloud<pcl::PointXYZ> error;
calibration::computeErrorPerPoint(tmpCloud,&error,center.head(3),ref,&valid);
//std::cout<<"error cloud has "<<error.size()<<" points"<<std::endl;
//std::cout<<"voxel cloud has "<<p2.pclCloud()->size()<<" points"<<std::endl;
pcl::PointCloud<pcl::PointXYZ>* cloudToCalibrate=p2.pclCloud();
//COMPUTE CALIBRATION MATRIX
std::cout.flush();
calibration::computeCalibrationMatrix(*cloudToCalibrate,error,k,&valid,c);
std::cout.flush();
//CALIBRATE POINT CLOUD
pcl::PointCloud<pcl::PointXYZ> fixedCloud;
calibration::calibratePointCloudWithMultipliers(*cloudToCalibrate,fixedCloud,c,k);
std::cout.flush();
error.clear();
valid.clear();
error.clear();
cloudToCalibrate->clear();
delete cloudToCalibrate;
tmpCloud->clear();
delete tmpCloud;
p2.cloud.clear();
normals->clear();
outFromNormalRejection.clear();
valid.clear();
delete normals;
delete square;
pcl->clear();
delete pcl;
}
std::cout<<std::endl<<"saving "<<std::endl;
c.dumpSensorImages();
c.serialize(argv[2]);
char nn[500];
std::cout<<"saving nn"<<std::endl;
sprintf(nn,"NN_%s",argv[2]);
//c.serializeNN(nn);
//calibrationMatrix* cc = c.downsample(2,2);
//cc->serialize(argv[2]);
}