bool SensorProcessorBase::process(
const pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr pointCloudInput,
const Eigen::Matrix<double, 6, 6>& robotPoseCovariance,
const pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudMapFrame,
Eigen::VectorXf& variances)
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointCloudSensorFrame(new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::copyPointCloud(*pointCloudInput, *pointCloudSensorFrame);
cleanPointCloud(pointCloudSensorFrame);
ros::Time timeStamp;
timeStamp.fromNSec(1000 * pointCloudSensorFrame->header.stamp);
if (!updateTransformations(pointCloudSensorFrame->header.frame_id, timeStamp)) return false;
if (!transformPointCloud(pointCloudSensorFrame, pointCloudMapFrame, mapFrameId_)) return false;
std::vector<pcl::PointCloud<pcl::PointXYZRGB>::Ptr> pointClouds({pointCloudMapFrame, pointCloudSensorFrame});
removePointsOutsideLimits(pointCloudMapFrame, pointClouds);
if (!computeVariances(pointCloudSensorFrame, robotPoseCovariance, variances)) return false;
return true;
}
/*
** Function called when a key is hit
*/
void KeyboardFunc(unsigned char key, int x, int y) {
//Map keys to move about the environment in the X, Y, Z direction
switch (key) {
case 'a' : moveX+=0.05;break;
case 'd' : moveX-=0.05;break;
case 'w' : moveZ+=0.5;break;
case 's' : moveZ-=0.5;break;
case 'e' : moveY+=0.05;break;
case 'q' : moveY-=0.05;break;
case '8' : if (freenect_sync_set_tilt_degs(tiltAngle+=5, 0)) exit(1);break;
case '2' : if (freenect_sync_set_tilt_degs(tiltAngle-=5, 0)) exit(1);break;
case '5' : if (freenect_sync_set_tilt_degs(tiltAngle=0, 0)) exit(1);break;
case 'h' : moveX=0;moveY=0;moveZ=-5;angleX=0;angleY=0;break;
case 'f' : command = command^1; printf("points deleted: %d\n", cleanPointCloud(rgbToWorld)); break; //freeze frame
case 'x' :
case 'X' :
case 27 :exit(0);
}
}
