void ExtractBuildPlane() {
std::vector<int> inliers; // build plane indices in the raw building point cloud
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
PlaneModel(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(build_cloud_raw)); // plane Model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(PlaneModel); // ransac
// set attributes...
ransac.setDistanceThreshold(0.01);
ransac.computeModel();
// get result
ransac.getInliers(inliers);
// copies all inliers of the model computed to another PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr build_cloud_plane(new pcl::PointCloud<pcl::PointXYZ>);
pcl::copyPointCloud<pcl::PointXYZ>(*build_cloud_raw, inliers, *build_cloud_plane);
// get plane model
ransac.getModelCoefficients(build_plane_coeff);
// project the points to the plane to reduce error
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
coefficients->values.resize (4);
coefficients->values[0] = build_plane_coeff(0);
coefficients->values[1] = build_plane_coeff(1);
coefficients->values[2] = build_plane_coeff(2);
coefficients->values[3] = build_plane_coeff(3);
pcl::ProjectInliers<pcl::PointXYZ> proj;
proj.setModelType(pcl::SACMODEL_PLANE);
proj.setInputCloud(build_cloud_plane);
proj.setModelCoefficients(coefficients);
proj.filter(*build_cloud_accurate_plane);
}
int
filterPlaneModel (pcl::PointCloud<pcl::PointXYZRGB>::Ptr &cloud, float distance)
{
Eigen::VectorXf planecoeffs, coeffs;
pcl::PointIndices::Ptr
inliers (new pcl::PointIndices);
pcl::SampleConsensusModelPlane<pcl::PointXYZRGB>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZRGB> (cloud));
pcl::ExtractIndices<pcl::PointXYZRGB> extract;
pcl::RandomSampleConsensus<pcl::PointXYZRGB> ransac (model_p);
ransac.setDistanceThreshold (distance);
ransac.computeModel ();
ransac.getInliers (inliers->indices);
ransac.getModelCoefficients (planecoeffs);
model_p->optimizeModelCoefficients (inliers->indices, planecoeffs, coeffs);
model_p->selectWithinDistance (coeffs, distance, inliers->indices);
if (inliers->indices.empty ())
return (-1);
// Remove the points that fit a plane from the cloud
pcl::ExtractIndices<pcl::PointXYZRGB> eifilter;
eifilter.setInputCloud (cloud);
eifilter.setIndices (inliers);
eifilter.setNegative (true);
eifilter.setKeepOrganized (true);
eifilter.filter (*cloud);
return (0);
}
//Finds the pole using the RANSAC algorithm
bool redblade_stereo::findPole(pcl::PointCloud<pcl::PointXYZ>::Ptr in,
pcl::PointCloud<pcl::PointXYZ>::Ptr pole){
ROS_INFO("Size of input cloud %d",in->points.size());
if(in->points.size()<sigSize){//Significance test
return false;
}
Eigen::VectorXf coeff;
coeff.resize(6);
ransac(in,pole,coeff);
ROS_INFO("Size of coefficients %d",coeff.size());
if(coeff.size()==0){
return false;
}
ROS_INFO("Model Coefficients (x:%f,y:%f,z:%f)+(dx:%f,dy:%f,dz:%f)",
coeff[0],coeff[1],coeff[2],coeff[3],coeff[4],coeff[5]);
if(fabs(coeff[5])>verticalTolerance){ //Vertical line test
if(pole->points.size()>sigSize){//Significance test
// int clusters = cluster(pole,0.01); //1 cm
// if(clusters==1){ //Test to see if the line is one contiguous segment
ROS_INFO("Size of line %d",pole->points.size());
// ROS_INFO("Model Coefficients (x:%f,y:%f,z:%f)+(dx:%f,dy:%f,dz:%f)",
// coeff[0],coeff[1],coeff[2],coeff[3],coeff[4],coeff[5]);
return true;}}
//}
return false;
}
Matrix33 RansacEstimator::getEssentialRansac(vector<Correspondance *> *data)
{
Ransac<Correspondance, ModelEssential8Point>
ransac(trySize);
ransac.data = data;
ransac.inlierThreshold = treshold;
ransac.inliersPercent = 1.0;
ransac.iterationsNumber = maxIterations;
ModelEssential8Point result = ransac.getModelRansac();
for (unsigned i = 0; i < data->size(); i++)
{
bool fits = result.fits(*(data->operator[](i)), treshold);
data->operator[](i)->flags |= (fits ? Correspondance::FLAG_PASSER : Correspondance::FLAG_FAILER);
}
for (unsigned i = 0; i < ransac.bestSamples.size(); i++)
{
ransac.bestSamples[i]->flags |= Correspondance::FLAG_IS_BASED_ON;
}
return result.model;
}
int ransac_vertrapezoid(int *matched_points, int npoints,
int *num_inliers_by_motion, double *params_by_motion,
int num_desired_motions) {
return ransac(matched_points, npoints, num_inliers_by_motion,
params_by_motion, num_desired_motions, 4,
is_degenerate_homography, find_vertrapezoid,
project_points_double_vertrapezoid);
}
int ransac_translation(int *matched_points, int npoints,
int *num_inliers_by_motion, double *params_by_motion,
int num_desired_motions) {
return ransac(matched_points, npoints, num_inliers_by_motion,
params_by_motion, num_desired_motions, 3,
is_degenerate_translation, find_translation,
project_points_double_translation);
}
/*!
Compute the pose from a set of n 2D point (x,y) in p and m 3D points
(X,Y,Z) in P using the Ransac algorithm. It is not assumed that
the 2D and 3D points are registred (there is nm posibilities)
at least numberOfInlierToReachAConsensus of true correspondance are required
to validate the pose
the inliers are given in a list of vpPoint
the pose is return in cMo
*/
void
vpPose::ransac(const int n,
const vpPoint *p,
const int m,
const vpPoint *P,
const int numberOfInlierToReachAConsensus,
const double threshold,
int &ninliers,
vpList<vpPoint> &lPi,
vpHomogeneousMatrix &cMo)
{
double *x, *y;
x = new double [n];
y = new double [n] ;
int i;
for (i=0 ; i < n ; i++)
{
x[i] = p[i].get_x() ;
y[i] = p[i].get_y() ;
}
double *X, *Y, *Z;
X = new double [m];
Y = new double [m];
Z = new double [m];
for (i=0 ; i < m ; i++)
{
X[i] = P[i].get_oX() ;
Y[i] = P[i].get_oY() ;
Z[i] = P[i].get_oZ() ;
}
vpColVector xi,yi,Xi,Yi,Zi ;
ransac(n,x,y,
m,X,Y,Z, numberOfInlierToReachAConsensus,
threshold,
ninliers,
xi,yi,Xi,Yi,Zi,
cMo) ;
for(i=0 ; i < ninliers ; i++)
{
vpPoint Pi ;
Pi.setWorldCoordinates(Xi[i],Yi[i], Zi[i]) ;
Pi.set_x(xi[i]) ;
Pi.set_y(yi[i]) ;
lPi += Pi ;
}
delete [] x;
delete [] y;
delete [] X;
delete [] Y;
delete [] Z;
}
void Segmentation::doSegmentation(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
//MainPlane groundMainPlane;
// Variables
isComputed = false ;
struct timeval tbegin,tend;
double texec=0.0;
// Start timer
gettimeofday(&tbegin,NULL);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFiltered(new pcl::PointCloud<pcl::PointXYZRGBA>);
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloudFilteredCopy(new pcl::PointCloud<pcl::PointXYZRGBA>);
_mainCloud.reset(new pcl::PointCloud<pcl::PointXYZRGBA>);
(void) passthroughFilter(-5.0, 5.0, -5.0, 5.0, -5.0, 5.0, cloud, cloudFiltered); // 3.0, 3.0, -0.2, 2.0, -4.0, 4.0
pcl::copyPointCloud(*cloudFiltered, *_mainCloud);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground (new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr Hullground (new pcl::PointCloud<pcl::PointXYZRGBA>);
std::vector<pcl::PointIndices> vectorInliers(0);
std::vector <pcl::ModelCoefficients> vectorCoeff(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorCloudinliers(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorHull(0);
std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > vectorCluster(0);
pcl::ModelCoefficients::Ptr coefficientsGround (new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliersGround (new pcl::PointIndices);
// pcl::VoxelGrid< pcl::PointXYZRGBA > sor;
// sor.setInputCloud (_mainCloud);
// sor.setLeafSize (0.01f, 0.01f, 0.01f);
// sor.filter (*_mainCloud);
//(void) findGround(_mainCloud,_ground);
//isComputed = true;
if(findGround(_mainCloud,_ground))
{
Eigen::Vector3f gravityVector(_ground.getCoefficients()->values[0], _ground.getCoefficients()->values[1], _ground.getCoefficients()->values[2]);
//(void) findOtherPlanesRansac(_mainCloud,gravityVector,vectorCoeff , vectorCloudinliers,vectorHull);
//(void) regionGrowing(vectorCloudinliers, vectorCluster);
euclidianClustering(_mainCloud,vectorCloudinliers);
ransac(vectorCloudinliers,gravityVector,vectorCluster );
createPlane(vectorCluster);
// End timer
}
gettimeofday(&tend,NULL);
// Compute execution time
texec = ((double)(1000*(tend.tv_sec-tbegin.tv_sec)+((tend.tv_usec-tbegin.tv_usec)/1000)))/1000;
std::cout << "time : " << texec << std::endl;
}
// The main function
int main(void)
{
// Generate noisy data from the truth
LineObserver observer;
std::vector<Point> data;
std::vector<bool> trueInliers;
Line trueModel(0.6, -0.8, 1);
if (!observer.GenerateData(data, trueInliers, trueModel, 100, 0.1, 0.5)) return -1;
// Find the best model using RANSAC
LineEstimator estimator;
RTL::RANSAC<Line, std::vector<Line>, Point, std::vector<Point> > ransac(&estimator);
Line bestModel;
double bestLoss = ransac.FindBest(bestModel, data, data.size());
// Find inliers using the best model
std::vector<bool> inliers;
int numInliers = ransac.FindInliers(inliers, bestModel, data, data.size());
// Print the data
std::ios_base::fmtflags original_flags = std::cout.flags();
std::cout << " No: x y true ransac xor" << std::endl;
for (size_t i = 0; i < data.size(); i++) {
std::cout.setf(std::ios_base::right, std::ios_base::adjustfield);
std::cout.width(3);
std::cout << i << ": ";
std::cout.width(8);
std::cout.setf(std::ios_base::right, std::ios_base::adjustfield);
std::cout << std::fixed << std::setprecision(2) << data[i].x << ", ";
std::cout.width(8);
std::cout.setf(std::ios_base::right, std::ios_base::adjustfield);
std::cout << std::fixed << std::setprecision(2) << data[i].y;
std::cout << " " << (trueInliers[i] ? "O" : "X");
std::cout << " " << (inliers[i] ? "O" : "X");
std::cout << " " << ((trueInliers[i] ^ inliers[i]) ? "*" : " ");
std::cout << std::endl;
}
std::cout.flags(original_flags); //6
// Print the result
std::cout << "- True Model: " << trueModel << std::endl;
std::cout << "- Found Model: " << bestModel << " (Loss: " << bestLoss << ")" << std::endl;
std::cout << "- The Number of Inliers: " << numInliers << std::endl;
return 0;
}
void Align::laserCallback(const sensor_msgs::LaserScan::ConstPtr& msg)
{
if(position == 4){
ransac();
}
if(ready == 1 && position < 4){
ROS_INFO("LaserCallBack Position: %d", position);
ready = 0;
for(int i = 0; i < 512; i++){
double dist = msg->ranges[i];
if(!isnan(dist) && dist != 0.0){
points.push_back(point());
points.at(counter).x = dist * cos(((M_PI / 511.0) * i) + (M_PI/2 * position));
points.at(counter).y = dist * sin(((M_PI / 511.0) * i) + (M_PI/2 * position));
switch(position){
case 0:
points[counter].y += 0.13;
break;
case 1:
points[counter].x -= 0.13;
break;
case 2:
points[counter].y -= 0.13;
break;
case 3:
points[counter].x += 0.13;
break;
default:
break;
}
//if(i % 4 == 0){
//ROS_INFO("PUSH i range: %d dist: %f x: %f y: %f", i, dist, points.at(counter).x,points.at(counter).y);}
counter++;
//counter++;
}
}
ROS_INFO("LaserCallBack points SIZE: %d", points.size());
position++;
rotateLeft(M_PI/2.0);
}
}
/*!
* \brief shot_detector::processCloud The service function. It process the clouds with shot and returns the pose of the object
* \param req
* \param res
* \return
*/
bool shot_detector::processCloud(apc_msgs::shot_detector_srv::Request &req, apc_msgs::shot_detector_srv::Response &res)
{
pcl_functions::convertMsg(req.targetcloud, model);
//loadModel(*model,"/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/optimized_poisson_textured_mesh.ply");
pcl_functions::convertMsg(req.pointcloud, scene);
//pcl::io::loadPCDFile("/home/niko/projects/apc/catkin/src/apc_ros/apc_object_detection/niko_file.pcd",*scene);
std::cerr << "Originally positions" << std::endl;
std::cerr << scene->points[1].x << std::endl;
std::cerr << scene->points[1].y << std::endl;
std::cerr << scene->points[1].z << std::endl;
//Downsample the model and the scene so they have rougly the same resolution
pcl::PointCloud<PointType>::Ptr scene_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(scene, scene_filter, voxel_sample_);
scene = scene_filter;
pcl::PointCloud<PointType>::Ptr model_filter (new pcl::PointCloud<PointType> ());
pcl_functions::voxelFilter(model, model_filter, voxel_sample_);
model = model_filter;
// Randomly select a couple of keypoints so we don't calculte descriptors for everything
sampleKeypoints(model, model_keypoints, model_ss_);
sampleKeypoints(scene, scene_keypoints, scene_ss_);
//Calculate the Normals
calcNormals(model, model_normals);
calcNormals(scene, scene_normals);
//Calculate the shot descriptors at each keypoint in the scene
calcSHOTDescriptors(model, model_keypoints, model_normals, model_descriptors);
calcSHOTDescriptors(scene, scene_keypoints, scene_normals, scene_descriptors);
ransac(rototranslations, model, scene);
// Compare descriptors and try to find correspondences
// compare(model_descriptors,scene_descriptors);
Eigen::Matrix4f pose;
// if(model_scene_corrs->size ()!=0){
//groupCorrespondences();
pose = refinePose(rototranslations, model, scene);
//}
std::cerr << pose << std::endl;
if(pose == Eigen::Matrix4f::Identity())
return false;
Eigen::Matrix4d md(pose.cast<double>());
Eigen::Affine3d affine(md);
geometry_msgs::Pose transform;
tf::poseEigenToMsg(affine, transform);
res.pose = transform;
return true;
}
void cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
// Do data processing here...
// run pass through filter to shrink point cloud
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_passthrough(new pcl::PointCloud<pcl::PointXYZRGB>);
//passthrough_z(input, cloud_passthrough);
passthrough_y(input,cloud_passthrough);
//passthrough_x(cloud_passthrough);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_outlier(new pcl::PointCloud<pcl::PointXYZRGB>);
passthrough_oulier(input,cloud_outlier);
pub_out.publish(*cloud_outlier);
// run ransac to find floor
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_projected(new pcl::PointCloud<pcl::PointXYZRGB>);
ransac(cloud_passthrough, cloud_projected);
pub.publish(*cloud_projected);
}
/*************************************************************************************************
* Compute Ground Plane and return transform to align axis with ground
* if debug show plane in vtkRenderWindow
*************************************************************************************************/
void compute_ground_plane_transform(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, Eigen::Matrix4f *RBT_pcl, int debug)
{
std::vector<int> inliers;
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_l(new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
Eigen::VectorXf model_coefficients(4);
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_l);
ransac.setDistanceThreshold (0.05);
ransac.computeModel();
ransac.getInliers(inliers);
model_l->optimizeModelCoefficients(inliers,model_coefficients,model_coefficients);
double nx,ny,nz,p;
double a = model_coefficients[0];
double b = model_coefficients[1];
double c = model_coefficients[2];
double d = model_coefficients[3];
nx = a / sqrt(a*a + b*b + c*c);
ny = b / sqrt(a*a + b*b + c*c);
nz = c / sqrt(a*a + b*b + c*c);
p = -d / sqrt(a*a + b*b + c*c);
// Compute Rigid Body transform between plane axis and coordinate system axes
pcl::PointCloud<pcl::PointXYZ> source_points;
pcl::PointCloud<pcl::PointXYZ> target_points;
target_points.push_back (pcl::PointXYZ (0,0,0));
target_points.push_back (pcl::PointXYZ (1,0,0));
target_points.push_back (pcl::PointXYZ (0,1,0));
source_points.push_back (pcl::PointXYZ (nx*p,ny*p,nz*p));
source_points.push_back (pcl::PointXYZ (nx*p+1,ny*p+0,nz*p+0));
source_points.push_back (pcl::PointXYZ (nx*p+nx,ny*p+ny,nz*p+nz));
pcl::registration::TransformationEstimationSVD<pcl::PointXYZ, pcl::PointXYZ> trans_pcl;
trans_pcl.estimateRigidTransformation(source_points,target_points,*RBT_pcl);
}
void RedlineExtractorNode::makeLineModels (void)
{
std::vector<int> ransacIndices;
pcl::PointCloud<PointT> ransacPoints;
// Setup ransac
pcl::SampleConsensusModelLine<PointT>::Ptr lineModel(new pcl::SampleConsensusModelLine<PointT> (this->poiCloud.makeShared()));
pcl::RandomSampleConsensus<PointT> ransac(lineModel);
// Extract line one if possible
ransac.setDistanceThreshold(this->params.ransacDistance);
this->models.lineOneFound = ransac.computeModel();
ransac.getInliers(ransacIndices);
ransac.getModelCoefficients(this->models.lineOne);
// Extract points from poiCloud to ransacLineOne
pcl::copyPointCloud<pcl::PointXYZ>(this->poiCloud, ransacIndices, this->ransacLineOne);
// Remove points from poiCloud according to indices
pcl::PointCloud<PointT>::iterator it;
for (int i = ransacIndices.size() - 1; i >= 0; i--)
{
it = this->poiCloud.begin() + ransacIndices.at(i);
this->poiCloud.erase(it);
}
// Extract line two if possible
pcl::SampleConsensusModelLine<PointT>::Ptr lineModel1(new pcl::SampleConsensusModelLine<PointT> (this->poiCloud.makeShared()));
pcl::RandomSampleConsensus<PointT> ransac1(lineModel1);
ransacIndices.clear();
ransac1.setDistanceThreshold(this->params.ransacDistance);
this->models.lineTwoFound = ransac1.computeModel();
ransac1.getInliers(ransacIndices);
ransac1.getModelCoefficients(this->models.lineTwo);
// Extract points from poiCloud to ransacLineTwo
pcl::copyPointCloud<pcl::PointXYZ>(this->poiCloud, ransacIndices, this->ransacLineTwo);
}
int main(int argc, char** argv)
{
int ratio = 3;
int kernel_size = 3;
int sigma = 200;
//connection to camera
CvCapture* capture = cvCaptureFromCAM(CV_CAP_ANY);
if(!capture)
{
std::cerr << "ERROR: capture is NULL\n";
getchar();
return -1;
}
/* ONLY NEEDED FOR TESTING PURPOSES
cvNamedWindow("Camera Image", CV_WINDOW_AUTOSIZE);
// Create a Trackbar for user to enter threshold
cv::createTrackbar("Min Threshold:","Camera Image" , &lowThreshold, max_lowThreshold, DummyCallback);
// Create a Trackbar for user to enter threshold
cv::createTrackbar("Hough Threshold:","Camera Image" , &houghThreshold, max_houghThreshold, DummyCallback);
// Create a Trackbar for user to enter threshold
cv::createTrackbar("Sigma:","Camera Image" , &sigma, 1000, DummyCallback);
cvNamedWindow("Edge Image", CV_WINDOW_AUTOSIZE);
*/
ros::init(argc,argv, "circle_publisher");
ros::NodeHandle nh;
//Let's publish messages as defined in /msg/cirlce.msg on a topic called 'detected_circles' with a max. buffer of 1000 messages
ros::Publisher circle_publisher = nh.advertise<circle_detection::circle_msg>("detected_circles",1000);
ros::Rate loop_rate(10);
//used to create an Image Id
size_t id_count = 0;
while(ros::ok)
{
// Get a frame
IplImage* frame = cvQueryFrame(capture);
if (!frame)
{
std::cerr << "ERROR: frame is null...\n" ;
getchar();
break;
}
// image id
id_count++;
cv::Mat src(frame);
cv::Mat src_gray;
cv::Mat dst;
cv::Mat detected_edges;
dst.create(src.size(), src.type());
// covert the image to gray
cvtColor(src,src_gray,CV_BGR2GRAY);
// Reduce the noise so we avoid false circle detection
GaussianBlur(src_gray, detected_edges, cv::Size(9, 9), sigma/100.0, 0);
equalizeHist(detected_edges, detected_edges);
// Canny detector
Canny(detected_edges, detected_edges, lowThreshold, lowThreshold*ratio, kernel_size);
// Using Canny's output as a mask, we display our result
dst = cv::Scalar::all(0);
src.copyTo(dst, detected_edges);
std::vector<Point> edgePoints;
// iterate through the pixels of the canny image
for(int j=0;j<detected_edges.cols;j++)
{
for(int i=0;i<detected_edges.rows;i++)
{
unsigned char &color = *(detected_edges.data+detected_edges.step*i + j*detected_edges.elemSize());
unsigned char &bb = *(src.data+src.step*i + j*src.elemSize());
unsigned char &gg = *(src.data+src.step*i + j*src.elemSize() + 1);
unsigned char &rr = *(src.data+src.step*i + j*src.elemSize() + 2);
// check if the pixel is black or white (only edges are white)
if(color)
{
int max = max3(rr,gg,bb);
int min = min3(rr,gg,bb);
int delta = max - min;
// check saturation (only colorfull circles will be detacted
if(delta > 20)
{
edgePoints.push_back(Point((double) j,(double) i));
}
else
{
// mark pixel as no longer relevant, i.e. the pixel isn't recognized as edge
color = 0;
}
}
}
}
std::vector<Circle> detectedCircles;
/* Apply the RANSAC algorithm to find circles in the camera image
* Paramters: sink vector, points, eps, iterations, minSupporters, maxCircles
*/
ransac(detectedCircles, edgePoints, 5.0, 10000, 100, 1);
/* ONLY NEDDED FOR TESTIGN PURPOSES
// Draw the circles detected
for(size_t i = 0; i < detectedCircles.size(); i++)
{
cv::Point center(cvRound(detectedCircles[i].center.x), cvRound(detectedCircles[i].center.y));
int radius = cvRound(detectedCircles[i].radius);
// assert(radius > 0);
// circle center
circle(src, center, 3, cv::Scalar(0,255,0), -1, 8, 0 );
// circle outline
circle(src, center, radius, cv::Scalar(0,0,255), 3, 8, 0 );
}
//Results
imshow("Camera Image", src);
imshow("Edge Image", detected_edges);
*/
for(size_t i=0;i < detectedCircles.size();i++)
{
circle_detection::circle_msg msg;
std::stringstream ss;
ss << "IMG" << id_count;
msg.image_id = ss.str();
unsigned int radius = cvRound(detectedCircles[i].radius);
msg.radius = radius;
msg.center_x = cvRound(detectedCircles[i].center.x) ;
msg.center_y = cvRound(detectedCircles[i].center.y);
circle_publisher.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
//Do not release the frame!
//If ESC key pressed, Key=0x10001B under OpenCV 0.9.7(linux version),
//remove higher bits using AND operator
//cvWaitKey() is used as delay between the frames
if ( (cvWaitKey(100) & 255) == 27 ) break;
}
/* ONLY NEEDED FOR TESTING PURPOSES
// Release the capture device housekeeping
cvReleaseCapture( &capture );
cvDestroyWindow( "Display Image" );
*/
return 0;
}
void Panorama::remove_outliers()
{
remove_too_few_match();
ransac();
fit_prob_model();
}
//**************************************************************************************************************************getMotion()
void PSMpositionNode::getMotion(const sensor_msgs::LaserScan& scan, std::vector <depthPoint> *depthLine)
{
ROS_DEBUG("Received kinectLine");
// **** attmempt to match the two scans
// PM scan matcher is used in the following way:
// The reference scan (prevPMScan_) always has a pose of 0
// The new scan (currPMScan) has a pose equal to the movement
// of the laser in the world frame since the last scan (btTransform change)
// The computed correction is then propagated using the tf machinery
if (!initialized_)
{
initialized_ = initialize(scan);
if (initialized_) ROS_INFO("Matcher Horizontal initialized");
return;
}
prevPMScan_->rx = 0;
prevPMScan_->ry = 0;
prevPMScan_->th = 0;
btTransform currWorldToBase;
btTransform change;
change.setIdentity();
// what odometry model to use
if (useTfOdometry_)
{
// get the current position of the base in the world frame
// if no transofrm is available, we'll use the last known transform
getCurrentEstimatedPose(currWorldToBase, scan);
change = laserToBase_ * prevWorldToBase_.inverse() * currWorldToBase * baseToLaser_;
}
else if (useImuOdometry_)
{
imuMutex_.lock();
double dTheta = currImuAngle_ - prevImuAngle_;
prevImuAngle_ = currImuAngle_;
change.getRotation().setRPY(0.0, 0.0, dTheta);
imuMutex_.unlock();
}
PMScan * currPMScan = new PMScan(scan.ranges.size());
rosToPMScan(scan, change, currPMScan);
try
{
matcher_.pm_psm(prevPMScan_, currPMScan);
}
catch(int err)
{
ROS_WARN("Error in scan matching");
delete prevPMScan_;
prevPMScan_ = currPMScan;
return;
};
// **** calculate change in position
// rotate by -90 degrees, since polar scan matcher assumes different laser frame
// and scale down by 100
double dx = currPMScan->ry / ROS_TO_PM;
double dy = -currPMScan->rx / ROS_TO_PM;
double da = currPMScan->th;
// change = scan match result for how much laser moved between scans,
// in the world frame
change.setOrigin(btVector3(dx, dy, 0.0));
btQuaternion q;
q.setRPY(0, 0, da);
change.setRotation(q);
// **** publish the new estimated pose as a tf
if(take_vicon == true) {
prevWorldToBase_.setOrigin(btVector3(pos_vicon[0],pos_vicon[1], pos_vicon[2]));
btQuaternion vicon_q(quat_vicon.x(),quat_vicon.y(),quat_vicon.z(),quat_vicon.w());
prevWorldToBase_.setRotation(vicon_q);
prevWorldToBase_.setIdentity();
take_vicon=false;
}
currWorldToBase = prevWorldToBase_ * baseToLaser_ * change * laserToBase_;
heightLine = ransac(depthLine);
frameP_ = worldFrame_;
if (publishTf_ ) publishTf (currWorldToBase, scan.header.stamp);
if (publishPose_) publishPose(currWorldToBase, scan.header.stamp, frameP_, heightLine);
// **** swap old and new
delete prevPMScan_;
prevPMScan_ = currPMScan;
prevWorldToBase_ = currWorldToBase;
}
void PSMpositionNode::getMotion_can(const sensor_msgs::LaserScan& scan, std::vector <depthPoint> *depthLine)//*********************************************getMotion_can()
{
ROS_DEBUG("Received scan");
// **** if this is the first scan, initialize and leave the function here
if (!initialized_can)
{
initialized_can = initializeCan(scan);
if (initialized_can) ROS_INFO("Matcher initialized");
return;
}
// **** attmempt to match the two scans
// CSM is used in the following way:
// The reference scan (prevLDPcan_) always has a pose of 0
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the world frame since the last scan (btTransform change)
// The computed correction is then propagated using the tf machinery
prevLDPScan_->odometry[0] = 0;
prevLDPScan_->odometry[1] = 0;
prevLDPScan_->odometry[2] = 0;
prevLDPScan_->estimate[0] = 0;
prevLDPScan_->estimate[1] = 0;
prevLDPScan_->estimate[2] = 0;
prevLDPScan_->true_pose[0] = 0;
prevLDPScan_->true_pose[1] = 0;
prevLDPScan_->true_pose[2] = 0;
btTransform currWorldToBase;
btTransform change;
change.setIdentity();
// what odometry model to use
if (useTfOdometry_)
{
// get the current position of the base in the world frame
// if no transofrm is available, we'll use the last known transform
getCurrentEstimatedPose(currWorldToBase, scan);
change = laserToBase_ * prevWorldToBase_.inverse() * currWorldToBase * baseToLaser_;
}
else if (useImuOdometry_)
{
imuMutex_.lock();
double dTheta = currImuAngle_ - prevImuAngle_;
prevImuAngle_ = currImuAngle_;
change.getRotation().setRPY(0.0, 0.0, dTheta);
imuMutex_.unlock();
}
geometry_msgs::Pose2D p;
tfToPose2D(change, p);
LDP currLDPScan = rosToLDPScan(scan, p);
input_.laser_ref = prevLDPScan_;
input_.laser_sens = currLDPScan;
input_.first_guess[0] = 0;
input_.first_guess[1] = 0;
input_.first_guess[2] = 0;
sm_icp(&input_, &output_);
if (!output_.valid)
{
ROS_WARN("Error in scan matching");
ld_free(prevLDPScan_);
prevLDPScan_ = currLDPScan;
return;
}
// **** calculate change in position
double dx = output_.x[0];
double dy = output_.x[1];
double da = output_.x[2];
// change = scan match result for how much laser moved between scans,
// in the world frame
change.setOrigin(btVector3(dx, dy, 0.0));
btQuaternion q;
q.setRPY(0, 0, da);
change.setRotation(q);
frameP_ = worldFrame_;
heightLine = ransac(depthLine);
// **** publish the new estimated pose as a tf
if(take_vicon == true) {
prevWorldToBase_.setOrigin(btVector3(pos_vicon[0],pos_vicon[1], pos_vicon[2]));
btQuaternion vicon_q(quat_vicon.x(),quat_vicon.y(),quat_vicon.z(),quat_vicon.w());
prevWorldToBase_.setRotation(vicon_q);
}
currWorldToBase = prevWorldToBase_ * baseToLaser_ * change * laserToBase_;
if (publishTf_ ) publishTf (currWorldToBase, scan.header.stamp);
if (publishPose_) publishPose(currWorldToBase, scan.header.stamp, frameP_, heightLine);
// **** swap old and new
ld_free(prevLDPScan_);
prevLDPScan_ = currLDPScan;
prevWorldToBase_ = currWorldToBase;
// **** timing information - needed for profiling only
}
int main_cases(int c, char *v[])
{
if (c != 6 && c != 7 && c != 8) {
fprintf(stderr, "usage:\n\t%s {line,aff,affn,fm} "
// 0 1
"ntrials maxerr minliers omodel [omask [oinliers]] <data\n",*v);
//2 3 4 5 6 7
return EXIT_FAILURE;
}
// parse input options
char *model_id = v[1];
int ntrials = atoi(v[2]);
float maxerr = atof(v[3]);
int minliers = atoi(v[4]);
char *filename_omodel = c > 5 ? v[5] : 0;
char *filename_omask = c > 6 ? v[6] : 0;
char *filename_inliers = c > 7 ? v[7] : 0;
// declare context variables
int modeldim, datadim, nfit;
ransac_error_evaluation_function *model_evaluation;
ransac_model_generating_function *model_generation;
ransac_model_accepting_function *model_acceptation = NULL;
void *user_data = NULL;
// fill context variables according to ransac case
if (0 == strcmp(model_id, "line")) {
datadim = 2;
modeldim = 3;
nfit = 2;
model_evaluation = distance_of_point_to_straight_line;
model_generation = straight_line_through_two_points;
} else if (0 == strcmp(model_id, "aff")) {
datadim = 4;
modeldim = 6;
nfit = 3;
model_evaluation = affine_match_error;
model_generation = affine_map_from_three_pairs;
} else if (0 == strcmp(model_id, "affn")) {
datadim = 4;
modeldim = 6;
nfit = 3;
model_evaluation = affine_match_error;
model_generation = affine_map_from_three_pairs;
model_acceptation = affine_map_is_reasonable;
} else if (0 == strcmp(model_id, "hom")) {
datadim = 4;
modeldim = 9;
nfit = 4;
model_evaluation = homographic_match_error;
model_generation = homography_from_four;
//model_acceptation = homography_is_reasonable;
model_acceptation = NULL;
} else if (0 == strcmp(model_id, "aff3d")) {
datadim = 6;
modeldim = 12;
nfit = 4;
model_evaluation = affine3d_match_error;
model_generation = affine3d_map_from_four_pairs;
} else if (0 == strcmp(model_id, "fm")) { // fundamental matrix
datadim = 4;
modeldim = 9;
nfit = 7;
model_evaluation = epipolar_error;
model_generation = seven_point_algorithm;
//model_acceptation = fundamental_matrix_is_reasonable;
} else if (0 == strcmp(model_id, "fmn")) { // fundamental matrix
int main_hack_fundamental_matrix(int,char*[]);
return main_hack_fundamental_matrix(c-1, v+1);
} else if (0 == strcmp(model_id, "fmnt")) { // fundamental matrix
int main_hack_fundamental_trimatrix(int,char*[]);
return main_hack_fundamental_trimatrix(c-1, v+1);
} else {
printf("unrecognized model \"%s\"\n", model_id);
return EXIT_FAILURE;
}
// read input data
int n;
float *data = read_ascii_floats(stdin, &n);
n /= datadim;
// call the ransac function to fit a model to data
float model[modeldim];
bool *mask = xmalloc(n * sizeof*mask);
int n_inliers = ransac(mask, model, data, datadim, n, modeldim,
model_evaluation, model_generation,
nfit, ntrials, minliers, maxerr,
model_acceptation, user_data);
// print a summary of the results
if (n_inliers > 0) {
printf("RANSAC found a model with %d inliers\n", n_inliers);
printf("parameters =");
for (int i = 0; i < modeldim; i++)
printf(" %g", model[i]);
printf("\n");
} else printf("RANSAC found no model\n");
// if requested, save the inlying data points
if (filename_inliers) {
FILE *f = xfopen(filename_inliers, "w");
for (int i = 0; i < n; i++)
if (mask[i]) {
for(int d = 0; d < datadim; d++)
fprintf(f,"%g ",data[i*datadim+d]);
fprintf(f,"\n");
}
xfclose(f);
}
// if requested, save the inlier mask
if (filename_omask) {
FILE *f = xfopen(filename_omask, "w");
for (int i = 0; i < n; i++)
fprintf(f, mask[i]?" 1":" 0");
fprintf(f, "\n");
xfclose(f);
}
// if requested, save the model
if (filename_omodel) {
FILE *f = xfopen(filename_omodel, "w");
for (int i = 0; i < modeldim; i++)
fprintf(f, "%lf%c", model[i], i==modeldim-1?'\n':' ');
xfclose(f);
}
return EXIT_SUCCESS;
}
void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices2;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients_plane (new pcl::ModelCoefficients ());
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers_plane (new pcl::PointIndices ());
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr plane_seg_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_RANSAC_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_whole_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr rest_cloud_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr whole_pc (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr ball_candidate_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr plane_seg_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_plane_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_RANSAC_output (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr remove_false_ball_candidate (new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> inliers;
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Pass through Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time pass_start = ros::Time::now();
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 2.5);
pass.filter (*passthrough_filtered);
passthrough_pub.publish(passthrough_filtered);
ros::Time pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for plane features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time plane_seg_start = ros::Time::now();
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*passthrough_filtered, *plane_seg_cloud);
// Optional
seg.setOptimizeCoefficients (true);
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setAxis(Eigen::Vector3f (0, -1, 0)); // best plane should be perpendicular to z-axis
seg.setMaxIterations (40);
seg.setDistanceThreshold (0.05);
seg.setInputCloud (plane_seg_cloud);
seg.segment (*inliers_plane, *coefficients_plane);
extract_indices.setInputCloud(plane_seg_cloud);
extract_indices.setIndices(inliers_plane);
extract_indices.setNegative(false);
extract_indices.filter(*plane_seg_output);
pcl::toROSMsg (*plane_seg_output, *plane_seg_output_cloud);
plane_pub.publish(plane_seg_output_cloud);
ros::Time plane_seg_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Extract rest plane and passthrough filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices.setNegative (true);
extract_indices.filter (*remove_plane_cloud);
plane_seg_cloud.swap (remove_plane_cloud);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*plane_seg_cloud, *rest_whole_cloud);
rest_whole_pub.publish(rest_whole_cloud); // 'rest_whole_cloud' substituted whole_pc
ros::Time rest_pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time find_ball_start = ros::Time::now();
int iter = 0;
while(iter < 5)
{
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SACSegmentation
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*rest_whole_cloud, *sphere_cloud);
ros::Time sphere_start = ros::Time::now();
// Optional
seg.setOptimizeCoefficients (false);
// Mandatory
seg.setModelType (pcl::SACMODEL_SPHERE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (10000);
seg.setDistanceThreshold (0.03);
seg.setRadiusLimits (0.12, 0.16);
seg.setInputCloud (sphere_cloud);
seg.segment (*inliers_sphere, *coefficients_sphere);
//std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
if (inliers_sphere->indices.empty())
std::cerr << "[[--Can't find the Sphere Candidate.--]]" << std::endl;
else {
extract_indices.setInputCloud(sphere_cloud);
extract_indices.setIndices(inliers_sphere);
extract_indices.setNegative(false);
extract_indices.filter(*sphere_output);
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_seg_pub.publish(sphere_output_cloud); // 'sphere_output_cloud' means ball candidate point cloud
}
ros::Time sphere_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features pcl::SampleConsensusModelSphere
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_start = ros::Time::now();
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_output));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*sphere_output, inliers, *sphere_RANSAC_output);
pcl::toROSMsg (*sphere_RANSAC_output, *sphere_RANSAC_output_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* To discriminate a ball
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
double w = 0;
w = double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height);
if (w > 0.9) {
BALL = true;
std::cout << "can find a ball" << std::endl;
true_ball_pub.publish(sphere_RANSAC_output_cloud);
break;
} else {
BALL = false;
std::cout << "can not find a ball" << std::endl;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Exclude false ball candidate
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//ros::Time rest_pass_start = ros::Time::now();
// Create the filtering object
// Remove the planar inliers, extract the rest
extract_indices2.setInputCloud(plane_seg_cloud);
extract_indices2.setIndices(inliers_sphere);
extract_indices2.setNegative (true);
extract_indices2.filter (*remove_false_ball_candidate);
sphere_RANSAC_output.swap (remove_false_ball_candidate);
// publish result of Removal the planar inliers, extract the rest
pcl::toROSMsg (*sphere_RANSAC_output, *ball_candidate_output_cloud);
rest_ball_candidate_pub.publish(ball_candidate_output_cloud);
rest_whole_cloud = ball_candidate_output_cloud;
}
iter++;
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time sphere_RANSAC_end = ros::Time::now();
}
ros::Time find_ball_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time whole_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "plane size : " << plane_seg_output_cloud->width * plane_seg_output_cloud->height << std::endl;
std::cout << "rest size : " << rest_whole_cloud->width * rest_whole_cloud->height << std::endl;
std::cout << "sphere size : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
std::cout << "sphere RANSAC size : " << sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height << " " << sphere_RANSAC_output->points.size() << std::endl;
std::cout << "sphereness : " << double(sphere_RANSAC_output_cloud->width * sphere_RANSAC_output_cloud->height)
/double(sphere_output_cloud->width * sphere_output_cloud->height) << std::endl;
std::cout << "model coefficient : " << coefficients_plane->values[0] << " "
<< coefficients_plane->values[1] << " "
<< coefficients_plane->values[2] << " "
<< coefficients_plane->values[3] << " " << std::endl;
printf("\n");
std::cout << "whole time : " << whole_end - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
std::cout << "passthrough time : " << pass_end - pass_start << " sec" << std::endl;
std::cout << "plane time : " << plane_seg_end - plane_seg_start << " sec" << std::endl;
std::cout << "rest and pass time : " << rest_pass_end - rest_pass_start << " sec" << std::endl;
std::cout << "find a ball time : " << find_ball_end - find_ball_start << " sec" << std::endl;
//std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
//std::cout << "sphere ransac time : " << sphere_RANSAC_end - sphere_RANSAC_start << " sec" << std::endl;
printf("\n----------------------------------------------------------------------------\n");
printf("\n");
}
/*!
Compute the pose from a list lp of 2D point (x,y) and a list lP 3D points
(X,Y,Z) in P using the Ransac algorithm. It is not assumed that
the 2D and 3D points are registred
At least numberOfInlierToReachAConsensus of true correspondance are required
to validate the pose
The inliers are given in a list of vpPoint lPi.
The pose is returned in cMo.
\param lp : List of 2d points (x and y attributes are used).
\param lP : List of 3d points (oX, oY and oZ attributes are used).
\param numberOfInlierToReachAConsensus : The minimum number of inlier to have
to consider a trial as correct.
\param threshold : The maximum error allowed between the 2d points and the
reprojection of its associated 3d points by the current pose (in meter).
\param ninliers : Number of inliers found for the best solution.
\param lPi : List of points (2d and 3d) that are inliers for the best solution.
\param cMo : The computed pose (best solution).
\param maxNbTrials : Maximum number of trials before considering a solution
fitting the required \e numberOfInlierToReachAConsensus and \e threshold
cannot be found.
*/
void
vpPose::ransac(std::list<vpPoint> &lp,
std::list<vpPoint> &lP,
const int numberOfInlierToReachAConsensus,
const double threshold,
unsigned int &ninliers,
std::list<vpPoint> &lPi,
vpHomogeneousMatrix &cMo,
const int maxNbTrials)
{
unsigned int i;
unsigned int n = lp.size() ;
unsigned int m = lP.size() ;
double *x, *y;
x = new double [n];
y = new double [n];
vpPoint pin ;
i = 0;
for (std::list<vpPoint>::const_iterator it = lp.begin(); it != lp.end(); ++it)
{
pin = *it;
x[i] = pin.get_x() ;
y[i] = pin.get_y() ;
++ i;
}
double *X, *Y, *Z;
X = new double [m];
Y = new double [m];
Z = new double [m];
i = 0;
for (std::list<vpPoint>::const_iterator it = lP.begin(); it != lP.end(); ++it)
{
pin = *it;
X[i] = pin.get_oX() ;
Y[i] = pin.get_oY() ;
Z[i] = pin.get_oZ() ;
++i;
}
vpColVector xi,yi,Xi,Yi,Zi ;
ransac(n,x,y,
m,X,Y,Z, numberOfInlierToReachAConsensus,
threshold,
ninliers,
xi,yi,Xi,Yi,Zi,
cMo, maxNbTrials) ;
for( i=0 ; i < ninliers ; i++)
{
vpPoint Pi ;
Pi.setWorldCoordinates(Xi[i],Yi[i], Zi[i]) ;
Pi.set_x(xi[i]) ;
Pi.set_y(yi[i]) ;
lPi.push_back(Pi);
}
delete [] x;
delete [] y;
delete [] X;
delete [] Y;
delete [] Z;
}
/*!
Compute the pose from a list lp of 2D point (x,y) and a list lP 3D points
(X,Y,Z) in P using the Ransac algorithm. It is not assumed that
the 2D and 3D points are registred
at least numberOfInlierToReachAConsensus of true correspondance are required
to validate the pose
the inliers are given in a list of lPi vpPoint
the pose is return in cMo
*/
void
vpPose::ransac(vpList<vpPoint> &lp,
vpList<vpPoint> &lP,
const int numberOfInlierToReachAConsensus,
const double threshold,
int &ninliers,
vpList<vpPoint> &lPi,
vpHomogeneousMatrix &cMo)
{
int n = lp.nbElement() ;
int m = lP.nbElement() ;
double *x, *y;
x = new double [n];
y = new double [n];
vpPoint pin ;
lp.front() ;
int i = 0 ;
while(!lp.outside())
{
pin = lp.value() ; lp.next() ;
x[i] = pin.get_x() ;
y[i] = pin.get_y() ;
i++ ;
}
double *X, *Y, *Z;
X = new double [m];
Y = new double [m];
Z = new double [m];
lP.front() ;
i = 0 ;
while(!lP.outside())
{
pin = lP.value() ; lP.next() ;
X[i] = pin.get_oX() ;
Y[i] = pin.get_oY() ;
Z[i] = pin.get_oZ() ;
i++ ;
}
vpColVector xi,yi,Xi,Yi,Zi ;
ransac(n,x,y,
m,X,Y,Z, numberOfInlierToReachAConsensus,
threshold,
ninliers,
xi,yi,Xi,Yi,Zi,
cMo) ;
for( i=0 ; i < ninliers ; i++)
{
vpPoint Pi ;
Pi.setWorldCoordinates(Xi[i],Yi[i], Zi[i]) ;
Pi.set_x(xi[i]) ;
Pi.set_y(yi[i]) ;
lPi += Pi ;
}
delete [] x;
delete [] y;
delete [] X;
delete [] Y;
delete [] Z;
}
void inliers::checkThings()
{
ransac();
std::cout<<std::endl<<" ransac finished start fundamental matrix approximation "<<std::endl;
estimateFundamentalMatrix();
}
int
main (int argc, char** argv)
{
if (pcl::console::find_argument (argc, argv, "-h") >= 0)
{
print_help (argv[0]);
return 0;
}
//pcl::PointCloud<pcl::PointXYZ>::Ptr initial_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//pcl::PointCloud<pcl::PointXYZ>::Ptr matched_cloud(new pcl::PointCloud<pcl::PointXYZ>);
//cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
std::vector<int> pcd_file_indices = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
if (pcd_file_indices.size () != 1)
{
std::cout << "Need input PCD file" << std::endl;
return (-1);
}
std::string pcd_file_name = argv[pcd_file_indices[0]];
std::cout << "Reading file \"" << pcd_file_name << "\"" << std::endl;
if (pcl::io::loadPCDFile<pcl::PointXYZ> (pcd_file_name, *initial_cloud) == -1) //* load the file
{
std::cerr << "Couldn't read file " << pcd_file_name << std::endl;
return (-1);
}
std::cout << "Loaded "
<< initial_cloud->width * initial_cloud->height
<< " data points from "
<< pcd_file_name
<< std::endl;
pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
if (pcl::console::find_argument (argc, argv, "-j") >= 0)
{
std::cout << "Just Visualize example" << std::endl;
viewer.runOnVisualizationThreadOnce(viewer_initial_cloud);
}
else if (pcl::console::find_argument (argc, argv, "-m") >= 0)
{
std::cout << "Match and visualize matched" << std::endl;
std::vector<int> inliers;
//boost::shared_ptr< std::vector<int> > inliers_ptr(inliers);
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_sphere(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (initial_cloud, true));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
ransac.setDistanceThreshold (2);
ransac.computeModel(1);
ransac.getInliers(inliers);
std::cout << "Found "
<< inliers.size()
<< " inliers with RANSAC "
<< std::endl;
pcl::copyPointCloud<pcl::PointXYZ>(*initial_cloud, inliers, *matched_cloud);
viewer.runOnVisualizationThreadOnce(viewer_matched_cloud);
}
else if (pcl::console::find_argument (argc, argv, "-e") >= 0)
{
std::cout << "Match and visualize remaining" << std::endl;
std::vector<int> inliers;
//boost::shared_ptr< std::vector<int> > inliers_ptr(inliers);
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_sphere(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (initial_cloud, true));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
ransac.setDistanceThreshold (2);
ransac.computeModel(1);
ransac.getInliers(inliers);
std::cout << "Found "
<< inliers.size()
<< " inliers with RANSAC "
<< std::endl;
pcl::PointIndices::Ptr inliers_ptr (new pcl::PointIndices());
inliers_ptr->indices = inliers;
pcl::ExtractIndices<pcl::PointXYZ> ei_filter (true);
ei_filter.setInputCloud (initial_cloud);
ei_filter.setIndices (inliers_ptr);
ei_filter.setNegative (true);
ei_filter.filter (*matched_cloud);
//ei_filter.setNegative (true);
//ei_filter.filterDirectly (initial_cloud);
//pcl::copyPointCloud<pcl::PointXYZ>(*initial_cloud, inliers, *matched_cloud);
viewer.runOnVisualizationThreadOnce(viewer_matched_cloud);
//viewer.runOnVisualizationThreadOnce(viewer_initial_cloud);
}
else if (pcl::console::find_argument (argc, argv, "-b") >= 0)
{
std::cout << "Match and visualize both" << std::endl;
std::vector<int> inliers;
//boost::shared_ptr< std::vector<int> > inliers_ptr(inliers);
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_sphere(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (initial_cloud, true));
double min_r = 0;
double max_r = 0;
model_sphere->setRadiusLimits(20, 120);
model_sphere->getRadiusLimits(min_r, max_r);
std::cout << "Min R = " << min_r
<< " Max R = " << max_r << std::endl;
//pcl::LeastMedianSquares<pcl::PointXYZ> ransac (model_sphere);
//pcl::MaximumLikelihoodSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
//pcl::MEstimatorSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
//pcl::RandomizedMEstimatorSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
//pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
//pcl::ProgressiveSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
pcl::RandomizedRandomSampleConsensus<pcl::PointXYZ> ransac (model_sphere);
ransac.setDistanceThreshold (3);
ransac.setMaxIterations(10000);
ransac.computeModel();
ransac.getInliers(inliers);
std::cout << "Found "
<< inliers.size()
<< " inliers with RANSAC "
<< std::endl;
/*
std::vector<int> inliers4;
inliers4.push_back(inliers[0]);
inliers4.push_back(inliers[1]);
inliers4.push_back(inliers[2]);
inliers4.push_back(inliers[3]);
Eigen::VectorXf model_coefficients;
for (size_t i = 4; i < (inliers.size() - 4); i++)
{
bool coeffs = model_sphere->computeModelCoefficients(inliers4, model_coefficients);
if (coeffs)
{
std::cout << "Valid coefficients: " << model_coefficients << std::endl;
//pcl::PointXYZ o;
o.x = model_coefficients[0];
o.y = model_coefficients[1];
o.z = model_coefficients[2];
r = model_coefficients[3];
//viewer.addSphere (o, r, "sphere", 0);
if (r > 20 && r < 120) break;
}
else
{
//std::cout << "Invalid coefficients: " << model_coefficients<< std::endl;
}
inliers4.pop_back();
inliers4.push_back(inliers[i]);
}
*/
//pcl::computeCentroid(*initial_cloud, inliers, o);
//r = 30;
Eigen::VectorXf model_coefficients;
ransac.getModelCoefficients(model_coefficients);
std::cout << "Valid coefficients: " << model_coefficients << std::endl;
o.x = model_coefficients[0];
o.y = model_coefficients[1];
o.z = model_coefficients[2];
r = model_coefficients[3];
pcl::PointIndices::Ptr inliers_ptr (new pcl::PointIndices());
inliers_ptr->indices = inliers;
pcl::ExtractIndices<pcl::PointXYZ> ei_filter (true);
ei_filter.setInputCloud (initial_cloud);
ei_filter.setIndices (inliers_ptr);
ei_filter.setNegative (false);
ei_filter.filter (*matched_cloud);
ei_filter.setNegative (true);
ei_filter.filter (*initial_cloud);
//ei_filter.setNegative (true);
//ei_filter.filterDirectly (initial_cloud);
//pcl::copyPointCloud<pcl::PointXYZ>(*initial_cloud, inliers, *matched_cloud);
viewer.runOnVisualizationThreadOnce(viewer_matched_cloud);
viewer.runOnVisualizationThreadOnce(viewer_initial_cloud);
}
else if (pcl::console::find_argument (argc, argv, "-a") >= 0)
{
//shapes = true;
std::cout << "Shapes visualisation example" << std::endl;
}
else if (pcl::console::find_argument (argc, argv, "-v") >= 0)
{
//viewports = true;
std::cout << "Viewports example" << std::endl;
}
else if (pcl::console::find_argument (argc, argv, "-i") >= 0)
{
//interaction_customization = true;
std::cout << "Interaction Customization example" << std::endl;
}
else
{
print_help (argv[0]);
return 0;
}
//pcl::copyPointCloud<pcl::PointXYZ>(*initial_cloud, inliers, *matched_cloud);
//pcl::IndicesPtr inliers_ptr(new std::vector<int>(inliers));
//indices_rem = eifilter.getRemovedIndices ();
// The indices_rem array indexes all points of cloud_in that are not indexed by indices_in
//ei_filter.setNegative (true);
//ei_filter.filter (*indices_out);
// Alternatively: the indices_out array is identical to indices_rem
//eifilter.setNegative (false);
//eifilter.setUserFilterValue (1337.0);
//eifilter.filterDirectly (cloud_in);
// This will directly modify cloud_in instead of creating a copy of the cloud
// It will overwrite all fields of the filtered points by the user value: 1337
/*
std::cout << "Extracted "
<< matched_cloud->width * matched_cloud->height
<< " data points with filter"
<< std::endl;
std::cout << "Left "
<< initial_cloud->width * initial_cloud->height
<< " data points"
<< std::endl;
*/
//pcl::visualization::CloudViewer viewer ("Simple Cloud Viewer");
//boost::function2<void, pcl::visualization::PCLVisualizer&, int> viewer_green_cloud_f = &viewer_green_cloud;
//viewer.runOnVisualizationThreadOnce(viewer_initial_cloud);
//viewer.runOnVisualizationThreadOnce(viewer_matched_cloud);
while (!viewer.wasStopped ())
{
}
return (0);
}
int LDWS(unsigned char *img, int width, int height, PointKAIST *pt_left, PointKAIST *pt_right)
{
int result = 0, result2 = 0, i = 0;
///////////calibration(img, width, height, pt_left, pt_right);
search_lane(img, width, height-bottom_margin, pt_left, pt_right);
ransac(img, pt_left, pt_right, 10);
lane_filtering(pt_left, pt_right, width, height);
// 일정 프레임(현재 100)까지 calibration을 수행하도록 하는 구문
if(calibration_frame < 100)
{
calibration(pt_left, pt_right, width, height);
calibration_frame++;
}
//result = warning_system2(pt_left, pt_right);
//tracking(pt_left, pt_right, width, height);
if(pt_left[0].x == 0 && pt_left[0].y == 0)
init(pt_left, pt_right);
if(pt_right[0].x == 0 && pt_right[0].y == 0)
init(pt_left, pt_right);
/**
// 기준선 그리기
for(i=0; i<width; i++)
{
img[horizontal_center*width+i-1] = 255;
img[horizontal_center*width+i] = 255;
img[horizontal_center*width+i+1] = 255;
}
for(i=0; i<height; i++)
{
img[i*width+vertical_center-1] = 255;
img[i*width+vertical_center] = 255;
img[i*width+vertical_center+1] = 255;
}
**/
//printf("noise : %f\n", get_noise(img, width, height, pt_left[1].x, pt_right[1].x, pt_left[1].y));
/*
//printf("%d ", pt_left[0].x);
if((pt_right[0].x - pt_left[0].x) > 300 && pt_right[0].x != 0 && pt_right[0].y != 0 && pt_left[0].x != 0 && pt_right[0].y != 0)
// 한쪽만 움직인 경우는 제외함
{
if((prev_prev_pt_left[0].x - pt_left[0].x) != 0 && (prev_prev_pt_right[0].x - pt_right[0].x) != 0)
//printf("%d %d\n", pt_left[0].x, pt_right[0].x);
// vertical_center로 교체됨
result = warning_system(vertical_center, (pt_right[0].x - pt_left[0].x) / 2 + pt_left[0].x, warning_val, pt_left[0].x, pt_right[0].x);
}
else
{
result = 0;
init(pt_left, pt_right);
}
*/
result = warning_system(vertical_center, (pt_right[0].x - pt_left[0].x) / 2 + pt_left[0].x, warning_val, pt_left[0].x, pt_right[0].x);
//draw_lane(img, width, height, pt_left[0].x, pt_left[0].y, pt_right[0].x, pt_right[0].y, pt_left[2].x, pt_left[2].y, pt_right[2].x, pt_right[2].y, pt_left, pt_right);
// 오른쪽
if(result==1)
{
// 튀는 경우 방지
if(start == 0 && prev_warning > 0)
{
//draw_warning(img, width, height);
result2 = result;
init(pt_left, pt_right);
}
else
result2 = 0;
}
// 왼쪽
else if(result==2)
{
if(start == 0 && prev_warning > 0)
{
//draw_warning2(img, width, height);
result2 = result;
init(pt_left, pt_right);
}
else
result2 = 0;
}
else
{
start = 0;
result = 0;
result2 = 0;
}
prev_prev_pt_left[0].x = pt_left[0].x;
prev_prev_pt_left[0].y = pt_left[0].y;
prev_prev_pt_left[1].x = pt_left[1].x;
prev_prev_pt_left[1].y = pt_left[1].y;
prev_prev_pt_left[2].x = pt_left[2].x;
prev_prev_pt_left[2].y = pt_left[2].y;
prev_prev_pt_right[0].x = pt_right[0].x;
prev_prev_pt_right[0].y = pt_right[0].y;
prev_prev_pt_right[1].x = pt_right[1].x;
prev_prev_pt_right[1].y = pt_right[1].y;
prev_prev_pt_right[2].x = pt_right[2].x;
prev_prev_pt_right[2].y = pt_right[2].y;
// draw_test(img, width, height);
//printf("warning : %d %d\n", prev_warning, result);
//오른쪽 차선 1
if(prev_warning == 1 && result == 2)
{
//draw_warning(img, width, height);
//init(pt_left, pt_right);
return 1;
}
// 왼쪽 차선 2
else if(prev_warning == 2 && result == 1)
{
//draw_warning2(img, width, height);
//init(pt_left, pt_right);
return 2;
}
prev_warning = result;
return 0;
}
int ransac_rotzoom(int *matched_points, int npoints, int *num_inliers_by_motion,
double *params_by_motion, int num_desired_motions) {
return ransac(matched_points, npoints, num_inliers_by_motion,
params_by_motion, num_desired_motions, 3, is_degenerate_affine,
find_rotzoom, project_points_double_rotzoom);
}
int
main(int argc, char** argv)
{
// initialize PointClouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
// populate our PointCloud with points
cloud->width = 500;
cloud->height = 1;
cloud->is_dense = false;
cloud->points.resize (cloud->width * cloud->height);
for (size_t i = 0; i < cloud->points.size (); ++i)
{
if (pcl::console::find_argument (argc, argv, "-s") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if (i % 5 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else if(i % 2 == 0)
cloud->points[i].z = sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
else
cloud->points[i].z = - sqrt( 1 - (cloud->points[i].x * cloud->points[i].x)
- (cloud->points[i].y * cloud->points[i].y));
}
else
{
cloud->points[i].x = 1024 * rand () / (RAND_MAX + 1.0);
cloud->points[i].y = 1024 * rand () / (RAND_MAX + 1.0);
if( i % 2 == 0)
cloud->points[i].z = 1024 * rand () / (RAND_MAX + 1.0);
else
cloud->points[i].z = -1 * (cloud->points[i].x + cloud->points[i].y);
}
}
std::vector<int> inliers;
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr
model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));
if(pcl::console::find_argument (argc, argv, "-f") >= 0)
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
else if (pcl::console::find_argument (argc, argv, "-sf") >= 0 )
{
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// creates the visualization object and adds either our orignial cloud or all of the inliers
// depending on the command line arguments specified.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument (argc, argv, "-f") >= 0 || pcl::console::find_argument (argc, argv, "-sf") >= 0)
viewer = simpleVis(final);
else
void callback(const sensor_msgs::PointCloud2::ConstPtr& cloud)
{
ros::Time whole_start = ros::Time::now();
ros::Time declare_types_start = ros::Time::now();
// filter
pcl::VoxelGrid<sensor_msgs::PointCloud2> voxel_grid;
pcl::PassThrough<sensor_msgs::PointCloud2> pass;
pcl::ExtractIndices<pcl::PointXYZ> extract_indices;
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
std::vector<int> inliers;
// The plane and sphere coefficients
pcl::ModelCoefficients::Ptr coefficients_sphere (new pcl::ModelCoefficients ());
// The plane and sphere inliers
pcl::PointIndices::Ptr inliers_sphere (new pcl::PointIndices ());
// The point clouds
sensor_msgs::PointCloud2::Ptr voxelgrid_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr passthrough_filtered (new sensor_msgs::PointCloud2);
sensor_msgs::PointCloud2::Ptr sphere_output_cloud (new sensor_msgs::PointCloud2);
// The PointCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_cloud (new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr sphere_output (new pcl::PointCloud<pcl::PointXYZ>);
ros::Time declare_types_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Create Voxel grid Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// voxel_grid.setInputCloud (cloud);
// voxel_grid.setLeafSize (0.01, 0.01, 0.01);
// voxel_grid.filter (*voxelgrid_filtered);
//
// // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
// pcl::fromROSMsg (*voxelgrid_filtered, *transformed_cloud);
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* Pass through Filtering
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time pass_start = ros::Time::now();
pass.setInputCloud (cloud);
pass.setFilterFieldName ("z");
pass.setFilterLimits (0, 3.5);
pass.filter (*passthrough_filtered);
pass.setInputCloud (passthrough_filtered);
pass.setFilterFieldName ("y");
pass.setFilterLimits (-0.1, 0.3);
pass.filter (*passthrough_filtered);
passthrough_pub.publish(passthrough_filtered);
ros::Time pass_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* for sphere features
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud
pcl::fromROSMsg (*passthrough_filtered, *sphere_cloud);
ros::Time sphere_start = ros::Time::now();
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ> (sphere_cloud));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_s);
ransac.setDistanceThreshold (.01);
ransac.computeModel();
ransac.getInliers(inliers);
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*sphere_cloud, inliers, *sphere_output);
pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
sphere_pub.publish(sphere_output_cloud);
// // Optional
// seg.setOptimizeCoefficients (true);
// // Mandatory
// seg.setModelType (pcl::SACMODEL_SPHERE);
// seg.setMethodType (pcl::SAC_RANSAC);
// seg.setMaxIterations (10000);
// seg.setDistanceThreshold (0.05);
// seg.setRadiusLimits (0, 0.15);
// seg.setInputCloud (sphere_cloud);
// seg.segment (*inliers_sphere, *coefficients_sphere);
// //std::cerr << "Sphere coefficients: " << *coefficients_sphere << std::endl;
//
//
// extract_indices.setInputCloud(sphere_cloud);
// extract_indices.setIndices(inliers_sphere);
// extract_indices.setNegative(false);
// extract_indices.filter(*sphere_output);
//
// if (sphere_output->points.empty ())
// std::cerr << "Can't find the sphere component." << std::endl;
//
// pcl::toROSMsg (*sphere_output, *sphere_output_cloud);
// sphere_pub.publish(sphere_output_cloud);
ros::Time sphere_end = ros::Time::now();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
* visualize normals
*/
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
// pcl::visualization::PCLVisualizer viewer("PCL Viewer");
// viewer.setBackgroundColor (0.0, 0.0, 0.5);
// viewer.addPointCloudNormals<pcl::PointXYZ,pcl::Normal>(sphere_cloud, cloud_normals3);
// viewer.spin();
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
ros::Time whole_end = ros::Time::now();
std::cout << "cloud size : " << cloud->width * cloud->height << std::endl;
std::cout << "pass_th size : " << passthrough_filtered->width * passthrough_filtered->height << std::endl;
std::cout << "sphere size : " << sphere_output_cloud->width * sphere_output_cloud->height << std::endl;
printf("\n");
std::cout << "whole time : " << whole_end - whole_start << " sec" << std::endl;
std::cout << "declare types time : " << declare_types_end - declare_types_start << " sec" << std::endl;
std::cout << "passthrough time : " << pass_end - pass_start << " sec" << std::endl;
std::cout << "sphere time : " << sphere_end - sphere_start << " sec" << std::endl;
printf("\n----------------------------------------------------------------------------\n");
printf("\n");
}
Line SegmentStitching::extractLineFromMeasurements(pcl::PointCloud<pcl::PointXYZ>::Ptr measurements,
float ransacThreshold,
std::vector<int>* inliers){
// Create the sample consensus object for the received cloud
pcl::SampleConsensusModelLine<pcl::PointXYZ>::Ptr
modelLine(new pcl::SampleConsensusModelLine<pcl::PointXYZ>(measurements));
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(modelLine);
ransac.setDistanceThreshold(ransacThreshold);
ransac.computeModel();
ransac.getInliers(*inliers);
// Vector of 6 points
// see http://docs.pointclouds.org/trunk/classpcl_1_1_sample_consensus_model_line.html
Eigen::VectorXf coeff;
ransac.getModelCoefficients(coeff);
for (int i = 0; i < coeff.size(); i++){
ROS_INFO("Coeff %d: %f", i, coeff[i]);
}
ROS_INFO("#Inliers: %d", (int)inliers->size());
// Project inliers onto the line so that we end up with a cloud where
// min+max can be found in order to define a line segment
pcl::PointCloud<pcl::PointXYZ>::Ptr projected(new pcl::PointCloud<pcl::PointXYZ>);
// project the inliers onto the model, without copying non-inliers over
modelLine->projectPoints(*inliers, coeff, *projected, false);
ROS_INFO("Projected size: %d", (int)projected->size());
pcl::PointCloud<pcl::PointXYZ>::iterator it = projected->begin();
// Segments have start and end points. A point is at the start or end of the
// line if one of its coordinates corresponds to the minimum or maximum
// value of either of the coordinate axes
int xmaxInd = -1;
int xminInd = -1;
int ymaxInd = -1;
int yminInd = -1;
float xmax = -std::numeric_limits<float>::max();
float ymax = -std::numeric_limits<float>::max();
float xmin = std::numeric_limits<float>::max();
float ymin = std::numeric_limits<float>::max();
for (; it != projected->end(); it++) {
// ROS_INFO_STREAM("Point " << (int)(it - projected->begin()) << ": " << *it);
// std::cout << "x: " << it->x << ", max: " << xmax << " x bigger? " << (it->x > xmax) << std::endl;
if (it->x > xmax){
xmax = it->x;
// index of this point is
xmaxInd = it - projected->begin();
//ROS_INFO("X max ind: %d with val %f", xmaxInd, xmax);
}
if (it->y > ymax){
ymax = it->y;
// index of this point is
ymaxInd = it - projected->begin();
//ROS_INFO("Y max ind: %d with val %f", ymaxInd, ymax);
}
if (it->x < xmin){
xmin = it->x;
// index of this point is
xminInd = it - projected->begin();
//ROS_INFO("X min ind: %d with val %f", xminInd, xmin);
}
if (it->y < ymin){
ymin = it->y;
// index of this point is
yminInd = it - projected->begin();
//ROS_INFO("Y min ind: %d with val %f", yminInd, ymin);
}
}
ROS_INFO("xmin ind %d, xmax ind %d, ymin ind %d, ymax ind %d",
xminInd, xmaxInd, yminInd, ymaxInd);
ROS_INFO("xmin %f, xmax %f, ymin %f, ymax %f",
xmin, xmax, ymin, ymax);
// If the x or y coordinates are identical, then you have a vertical or
// horizontal line - define start and end points of the line with the value
// of xmin or max, and then assign values for the other coordinate to the
// start and end point arbitrarily. You should be able to do this min/max
// comparison based on the indices of xmin and xmax - they should be the same
// if lines are horizontal or vertical
if (xminInd == xmaxInd){
return Line(pcl::PointXYZ(xmin, ymin, 0), pcl::PointXYZ(xmin, ymax, 0));
} else if (yminInd == ymaxInd){
return Line(pcl::PointXYZ(xmin, ymin, 0), pcl::PointXYZ(xmax, ymin, 0));
} else {
// TODO: Make sure this works correctly
// This is a line which is not horizontal or vertical - the indices of
// xmin,ymin or xmax, ymax should match.
if (xminInd == yminInd && xmaxInd == ymaxInd){
return Line(pcl::PointXYZ(xmin, ymin, 0), pcl::PointXYZ(xmax, ymax, 0));
} else if (xmaxInd == yminInd && xminInd == ymaxInd){
return Line(pcl::PointXYZ(xmax, ymin, 0), pcl::PointXYZ(xmin, ymax, 0));
}
}
ROS_ERROR("Reached end of extract line without getting anything!");
}
int main(int argc, char** argv) {
//check if number of arguments is proper
if(argc!=3){
help();
exit(0);
}
// initialize PointClouds
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ>(argv[1], *cloud) == -1) //* load the file
{
cerr << "Couldn't read file :" << argv[1] << "\n";
return (-1);
}
std::vector<int> inliers;
// created RandomSampleConsensus object and compute the appropriated model
pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
//RANSAC for Line model
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_line(new pcl::PointCloud<PointT>());
if (pcl::console::find_argument(argc, argv, "-l") >= 0) {
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers_l(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZ> seg;
// Optional
seg.setOptimizeCoefficients(true);
// Mandatory
seg.setModelType(pcl::SACMODEL_LINE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(0.05);
seg.setInputCloud(cloud);
seg.segment(*inliers_l, *coefficients);
// Write the line inliers to disk
pcl::ExtractIndices<PointT> extract;
extract.setInputCloud(cloud);
extract.setIndices(inliers_l);
extract.setNegative(false);
extract.filter(*cloud_line);
}
if (pcl::console::find_argument(argc, argv, "-f") >= 0) {
//RANSAC for Plane model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);
ransac.setDistanceThreshold(.01);
ransac.computeModel();
ransac.getInliers(inliers);
} else if (pcl::console::find_argument(argc, argv, "-sf") >= 0) {
//RANSAC for Sphere model
pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
ransac.setDistanceThreshold(.01);
ransac.computeModel();
ransac.getInliers(inliers);
}
// copies all inliers of the model computed to another PointCloud
pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);
// creates the visualization object and adds either our orignial cloud or all of the inliers
// depending on the command line arguments specified.
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
if (pcl::console::find_argument(argc, argv, "-f") >= 0 || pcl::console::find_argument(argc, argv, "-sf") >= 0) {
viewer = simpleVis(final);
} else if (pcl::console::find_argument(argc, argv, "-l") >= 0) {
