float Projector_Calibrator::fitPlaneToCloud(const Cloud& cloud, Eigen::Vector4f& model){
// ROS_INFO("Fitting plane to cloud with %zu points", cloud.size());
if (cloud.size() < 1000){
ROS_WARN("fitPlaneToCloud: cloud size very small: %zu", cloud.size());
}
// http://pointclouds.org/documentation/tutorials/random_sample_consensus.php#random-sample-consensus
pcl::SampleConsensusModelPlane<pcl_Point>::Ptr
model_p (new pcl::SampleConsensusModelPlane<pcl_Point> (cloud.makeShared()));
pcl::RandomSampleConsensus<pcl_Point> ransac(model_p);
ransac.setDistanceThreshold(0.005); // max dist of 5mm
ransac.computeModel();
Eigen::VectorXf c;
ransac.getModelCoefficients(c);
model = c;
std::vector<int> inliers;
ransac.getInliers(inliers);
float inlier_pct = inliers.size()*100.0/cloud.size();
if (inlier_pct<0.5){ ROS_WARN("Only %.3f %% inlier in fitPlaneToCloud!", inlier_pct); }
return inlier_pct;
}
int main(int argc, char **argv) {
if(argc!=3){
printf("graphFindLeaves graph.gr leaves.cl\n");
exit(0);
}
Graph<Point3D, EdgeW<Point3D> >* gr =
new Graph<Point3D, EdgeW<Point3D> >(argv[1]);
Cloud<Point3D>* cl = new Cloud<Point3D>();
vector<int> pointsToAdd = gr->findLeaves();
for(int i = 0; i < pointsToAdd.size(); i++){
cl->addPoint(gr->cloud->points[pointsToAdd[i]]->coords[0],
gr->cloud->points[pointsToAdd[i]]->coords[1],
gr->cloud->points[pointsToAdd[i]]->coords[2]);
}
//Leaves are green
cl->v_r = 0;
cl->v_g = 1;
cl->v_b = 0;
cl->v_radius = 0.8;
cl->saveToFile(argv[2]);
}
/* ******************************************************************************************** */
int main () {
for(int i = 0; i < 4; i++) cs.push_back(Cloud());
pthread_mutex_init(&lock, NULL);
SimpleOpenNIViewer v1(1), v2(2);
v1.init();
v2.init();
// Create the viewer
Viewer* viewer = new Viewer ("3D Viewer");
viewer->setBackgroundColor (0, 0, 0);
viewer->addCoordinateSystem (1.0);
viewer->initCameraParameters ();
viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3,"sample");
// Visualize
while(true) {
Cloud c;
pthread_mutex_lock(&lock);
for(int i = 0; i < 2; i++) {
printf("%d\t", cs[i].size());
c += (cs[i]);
}
printf("\n");
pthread_mutex_unlock(&lock);
viewer->removePointCloud();
Cloud::Ptr cp = c.makeShared();
pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGBA> rgb(cp);
viewer->addPointCloud(cp, rgb);
viewer->spinOnce();
// printf("%d\n", c.size());
}
}
int main(int argc, char **argv) {
if(argc!= 5){
printf("Usage: diademAddScaleFromImage cloud.cl image cube_to_translate out.cl\n");
exit(0);
}
Cloud<Point3D>* orig = new Cloud<Point3D>(argv[1]);
Image<float>* scales = new Image<float> (argv[2]);
CubeF* cb = new CubeF (argv[3]);
Cloud<Point3Dw>* dest = new Cloud<Point3Dw>();
int x, y, z, nP;
for(nP = 0; nP < orig->points.size(); nP++){
cb->micrometersToIndexes3
(orig->points[nP]->coords[0], orig->points[nP]->coords[1], orig->points[nP]->coords[2],
x, y, z);
dest->points.push_back
(new Point3Dw(orig->points[nP]->coords[0],
orig->points[nP]->coords[1],
orig->points[nP]->coords[2],
scales->at(x,y)));
}
dest->saveToFile(argv[4]);
}
void Foam::ThermoParcel<ParcelType>::readFields(Cloud<ParcelType>& c)
{
if (!c.size())
{
return;
}
KinematicParcel<ParcelType>::readFields(c);
IOField<scalar> T(c.fieldIOobject("T", IOobject::MUST_READ));
c.checkFieldIOobject(c, T);
IOField<scalar> cp(c.fieldIOobject("cp", IOobject::MUST_READ));
c.checkFieldIOobject(c, cp);
label i = 0;
forAllIter(typename Cloud<ParcelType>, c, iter)
{
ThermoParcel<ParcelType>& p = iter();
p.T_ = T[i];
p.cp_ = cp[i];
i++;
}
}
// 初期化
bool TrophyNotification::init(const string& message)
{
if(!NotificationNode::init(5.f)) return false;
// ラベルを生成
Label* messageL {Label::createWithTTF("トロフィを獲得しました", Resource::Font::SYSTEM, 20.f)};
Label* trophyName { Label::createWithTTF(message, Resource::Font::SYSTEM, 25.f) };
// トロフィ生成
Sprite* trophy { Sprite::createWithSpriteFrameName("trophy_gold.png") };
float trophyScale { (messageL->getContentSize().height + trophyName->getContentSize().height) * 0.6f / trophy->getContentSize().height};
trophy->setScale(trophyScale);
// 背景を生成
Cloud* bg { Cloud::create(Size(trophy->getContentSize().width * trophyScale + max(trophyName->getContentSize().width, messageL->getContentSize().width), trophyName->getContentSize().height)) };
this->addChild(bg);
this->addChild(messageL);
this->addChild(trophyName);
this->addChild(trophy);
messageL->setPosition(trophy->getContentSize().width * trophyScale / 2, messageL->getContentSize().height / 2 + 5.f);
trophyName->setPosition(trophy->getContentSize().width * trophyScale / 2, -messageL->getContentSize().height / 2);
trophy->setPosition(-bg->getContentSize().width / 2 + trophy->getContentSize().width * trophyScale, 0);
this->setDefaultPosition(Point(WINDOW_WIDTH - bg->getContentSize().width / 2, WINDOW_HEIGHT - bg->getContentSize().height / 2));
return true;
}
void Elevation_map::getModel(Cloud& model){
model.clear();
pcl_Point p;
model.resize(cell_cnt_y*cell_cnt_x);
int pos = 0;
for (int y = 0; y < cell_cnt_y; ++y){
p.y = y_min_+(y+0.5)*cell_size_;
for (int x = 0; x < cell_cnt_x; ++x)
{
p.x = x_min_+(x+0.5)*cell_size_;
p.z = mean.at<float>(y,x);
// model.push_back(p);
model[pos++] = p;
}
}
model.width = cell_cnt_x;
model.height = cell_cnt_y;
if (int(model.size()) != cell_cnt_x*cell_cnt_y){
ROS_INFO("size: %zu, %i % i", model.size(),cell_cnt_x,cell_cnt_y);
// assert(int(model.size()) == cell_cnt_x*cell_cnt_y);
}
}
int main(int argc, char* argv[])
{
Cloud* cloudy = new Cloud(argv[1]);
/*j
//cloudy->sphere(vec3(50, 50, 0), 100, 1.0, -1.0);
//cloudy->sphere(vec3(0, 100, 0), 42, (1), (1));
//cloudy->sphere(vec3(30, 100, 0), 30, (-1), (-1));
//cloudy->noisySphere(vec3(0, 100, 0), 100, 1.0, 1.0);
//cloudy->cube(vec3(0, 0, 42), vec3(42, 42, 45), 1.0, 0);
//cloudy->cube(vec3(0, 0, 0), vec3(42, 42, 3), 1.0, 0);
//cloudy->cube(vec3(25, 25, 25), vec3(75, 75, 75), 0.5, 0.5);
//cloudy->cube(vec3(26, 26, 26), vec3(74, 74, 74), -1, -1);
//cloudy->noisySphere(vec3(50, 50, 50), 25, 1.0, 1.0);
//cloudy->cube(vec3(15, 15, 1), vec3(25, 25, 99), -1, -1);
cloudy->sphere(vec3(70, 70, 30), 40, 1, 1);
cloudy->cube(vec3(30, 30, 30), vec3(70, 70, 70), 1.0, 0);
cloudy->cube(vec3(31, 31, 31), vec3(69, 69, 69), -1.0, -1.0);
cloudy->noisyCube(vec3(30, 30, 30), vec3(70, 70, 70), 1.0, -1.0);
*/
cloudy->render(0);
delete cloudy;
return 0;
}
void ChangeDisplay::addColoredCloud( Cloud& cloud, int r, int g, int b, int alpha) {
for(Cloud::iterator pt = cloud.begin(); pt < cloud.end(); pt++) {
pt->r = (r > 0) ? r : pt->r;
pt->g = (g > 0) ? g : pt->g;
pt->b = (b > 0) ? b : pt->b;
pt->a = alpha;
}
display_cloud += cloud;
}
void Foam::IOPosition<ParticleType>::readData
(
Cloud<ParticleType>& c,
bool checkClass
)
{
Istream& is = readStream(checkClass ? typeName : "");
token firstToken(is);
if (firstToken.isLabel())
{
label s = firstToken.labelToken();
// Read beginning of contents
is.readBeginList("Cloud<ParticleType>");
for (label i=0; i<s; i++)
{
// Do not read any fields, position only
c.append(new ParticleType(c, is, false));
}
// Read end of contents
is.readEndList("Cloud<ParticleType>");
}
else if (firstToken.isPunctuation())
{
if (firstToken.pToken() != token::BEGIN_LIST)
{
FatalIOErrorIn
(
"void IOPosition<ParticleType>::readData"
"(Cloud<ParticleType>&, bool)",
is
) << "incorrect first token, '(', found "
<< firstToken.info()
<< exit(FatalIOError);
}
token lastToken(is);
while
(
!(
lastToken.isPunctuation()
&& lastToken.pToken() == token::END_LIST
)
)
{
is.putBack(lastToken);
// Do not read any fields, position only
c.append(new ParticleType(c, is, false));
is >> lastToken;
}
}
else
{
Cloud* Cloud::createWithSpriteFrame(cocos2d::CCSpriteFrame* pSpriteFrame)
{
Cloud* pobCloud = new Cloud();
if (pSpriteFrame && pobCloud && pobCloud->initWithSpriteFrame(pSpriteFrame))
{
pobCloud->autorelease();
return pobCloud;
}
CC_SAFE_DELETE(pobCloud);
return NULL;
}
Cloud* Cloud::create(const char* pszFileName, const CCRect& rect)
{
Cloud* pobCloud = new Cloud();
if (pobCloud && pobCloud->initWithFile(pszFileName, rect))
{
pobCloud->autorelease();
return pobCloud;
}
CC_SAFE_DELETE(pobCloud);
return NULL;
}
void GameLayer::generateClouds(){
//random number of clouds
int cloudsCount = 15;
for(int i = 0 ; i < cloudsCount ; i++ )
{
Cloud * cloud = new Cloud( this, m_world, worldStartX, worldEndX, perspectiveX );
cloud->createCloud( Point( -worldEndX/3+rand()%(int)(1.3*(int)worldEndX), 350+(rand()%(int)(winSize.height-200)) ) );
clouds.push_back(cloud);
}
}
void vm::scanner::cuda::renderImage(const Cloud& points, const Normals& normals, const Intr& intr, const Vec3f& light_pose, Image& image)
{
image.create(points.rows(), points.cols());
const device::Points& p = (const device::Points&)points;
const device::Normals& n = (const device::Normals&)normals;
device::Reprojector reproj(intr.fx, intr.fy, intr.cx, intr.fy);
device::Vec3f light = device_cast<device::Vec3f>(light_pose);
device::Image& i = (device::Image&)image;
device::renderImage(p, n, reproj, light, i);
waitAllDefaultStream();
}
void vm::scanner::cuda::resizePointsNormals(const Cloud& points, const Normals& normals, Cloud& points_out, Normals& normals_out)
{
points_out.create (points.rows()/2, points.cols()/2);
normals_out.create (normals.rows()/2, normals.cols()/2);
device::Points& pi = (device::Points&)points;
device::Normals& ni= (device::Normals&)normals;
device::Points& po = (device::Points&)points_out;
device::Normals& no = (device::Normals&)normals_out;
device::resizePointsNormals(pi, ni, po, no);
}
void MyController::onSpecialKeyReleased(int key) {
if (key == GLUT_KEY_F1) {
cg::Manager::instance()->getApp()->dump();
}
if (key == GLUT_KEY_F2) {
MyTeapot *box = (MyTeapot*)cg::Registry::instance()->get("Teapot1");
if(box) {
box->toggleDebugMode();
}
MyLight *light = (MyLight*)cg::Registry::instance()->get("Light1");
if(light) {
light->toggleDebugMode();
}
MyBox *obj = (MyBox*)cg::Registry::instance()->get("Boxmagica");
if(obj) {
obj->toggleDebugMode();
}
Cloud *cl = (Cloud*)cg::Registry::instance()->get("Nuvem");
if(cl) {
cl->toggleDebugMode();
}
MyFPSCamera *fpscamera = (MyFPSCamera*)cg::Registry::instance()->get("FPSCamera");
if(fpscamera->isActive()) {
fpscamera->toggleFPSMode();
MyCamera *camera = (MyCamera*)cg::Registry::instance()->get("Camera");
if(camera){
camera->toggleTopMode();
camera->toggleDebugMode(); //pode dar asneirada, sai de debugmode como?
}
}
}
if (key == GLUT_KEY_F3) {
MyFPSCamera *fpscamera = (MyFPSCamera*)cg::Registry::instance()->get("FPSCamera");
MyCamera *camera = (MyCamera*)cg::Registry::instance()->get("Camera");
MyBox *obj = (MyBox*)cg::Registry::instance()->get("Boxmagica");
Cloud *cl = (Cloud*)cg::Registry::instance()->get("Nuvem");
cl->togglemovable();
obj->togglemovable();
if(camera){
camera->toggleTopMode();
}
if(fpscamera){
fpscamera->toggleFPSMode();
}
}
}
pcl2::Neighborhood
pcl2::computeFixedRadiusNeighborhood (Cloud & cloud, const MatF & query, float r)
{
// Convert point cloud
MatF xyz = cloud["xyz"];
assert (xyz.rows () >= 1);
assert (xyz.cols () == 3);
pcl::PointCloud<pcl::PointXYZ>::Ptr input (new pcl::PointCloud<pcl::PointXYZ>);
input->width = cloud.size ();
input->height = 1;
input->is_dense = false;
input->points.resize (cloud.size ());
for (size_t i = 0; i < xyz.rows (); ++i)
{
input->points[i].x = xyz (i, 0);
input->points[i].y = xyz (i, 1);
input->points[i].z = xyz (i, 2);
}
// Convert query point
assert (query.rows () == 1);
assert (query.cols () == 3);
pcl::PointXYZ q;
q.x = query (0, 0);
q.y = query (0, 1);
q.z = query (0, 2);
// Perform neighbor search
pcl::KdTreeFLANN<pcl::PointXYZ> tree;
tree.setInputCloud (input);
std::vector<int> idx_vec;
std::vector<float> dist_vec;
size_t k = (size_t) tree.radiusSearch (q, r, idx_vec, dist_vec);
assert (k == idx_vec.size ());
// Convert output
EigenMat<int> neighbor_indices (k, 1);
EigenMat<float> squared_distances (k, 1);
for (size_t i = 0; i < k; ++i)
{
neighbor_indices (i, 0) = idx_vec[i];
squared_distances (i, 0) = dist_vec[i];
}
//Cloud neighborhood = cloud (neighbor_indices);
Neighborhood neighborhood (cloud, neighbor_indices);
neighborhood.insert ("dist", squared_distances);
return (neighborhood);
}
template<typename PointT, typename PointNT> void
pcl::CovarianceSampling<PointT, PointNT>::applyFilter (Cloud &output)
{
std::vector<int> sampled_indices;
applyFilter (sampled_indices);
output.resize (sampled_indices.size ());
output.header = input_->header;
output.height = 1;
output.width = uint32_t (output.size ());
output.is_dense = true;
for (size_t i = 0; i < sampled_indices.size (); ++i)
output[i] = (*input_)[sampled_indices[i]];
}
int main(int argc, char **argv) {
struct arguments arguments;
/* Default values. */
arguments.verbose = 0;
arguments.outputFile = "out.cl";
arguments.cloudName = "";
arguments.volumeX = "";
arguments.volumeY = "";
arguments.volumeZ = "";
argp_parse (&argp, argc, argv, 0, 0, &arguments);
printf ("Cloud = %s\n OutputFile = %s\n",
arguments.cloudName.c_str(), arguments.outputFile.c_str());
//Code starts here
Cloud<Point3D>* cl = new Cloud<Point3D>(arguments.cloudName);
Cloud<Point3Do>* out = new Cloud<Point3Do>(arguments.outputFile);
Cube< float, double>* vx = new Cube<float, double>(arguments.volumeX);
Cube< float, double>* vy = new Cube<float, double>(arguments.volumeY);
Cube< float, double>* vz = new Cube<float, double>(arguments.volumeZ);
vector< int > idx(3);
vector< float > micr(3);
float theta, phi, r;
for(int i = 0; i < cl->points.size(); i++){
micr[0] = cl->points[i]->coords[0];
micr[1] = cl->points[i]->coords[1];
micr[2] = cl->points[i]->coords[2];
vx->micrometersToIndexes(micr, idx);
r = sqrt( vx->at(idx[0],idx[1],idx[2])*vx->at(idx[0],idx[1],idx[2]) +
vy->at(idx[0],idx[1],idx[2])*vy->at(idx[0],idx[1],idx[2]) +
vz->at(idx[0],idx[1],idx[2])*vz->at(idx[0],idx[1],idx[2]) );
theta = atan2(vy->at(idx[0],idx[1],idx[2]), vx->at(idx[0],idx[1],idx[2]));
phi = acos(vz->at(idx[0],idx[1],idx[2])/r);
Point3Do* pt = new Point3Do(micr[0], micr[1], micr[2],
theta, phi);
out->points.push_back(pt);
}
out->saveToFile(arguments.outputFile);
}
/// point is foreground if it is not within N sigma
void PixelEnvironmentModel::getForeground_prob(const Cloud& cloud, float N, cv::Mat& foreground){
if (foreground.rows != height_ || foreground.cols != width_ || foreground.type() != CV_8UC1){
foreground = cv::Mat(height_,width_,CV_8UC1);
}
foreground.setTo(0);
for (int y=0; y<height_; ++y)
for (int x=0; x<width_; ++x){
if (mask_set && mask_.at<uchar>(y,x) == 0) continue;
bool inited = gaussians[y][x].initialized;
float current = norm(cloud.at(x,y));
if (current < 0) continue; // nan point
if (!inited || (current < gaussians[y][x].mean && !gaussians[y][x].isWithinNSigma(current,N))){
foreground.at<uchar>(y,x) = 255;
}
}
cv::medianBlur(foreground,foreground,3);
// cv::erode(foreground,foreground,cv::Mat(),cv::Point(-1,-1),2);
// cv::dilate(foreground,foreground,cv::Mat(),cv::Point(-1,-1),2);
}
void Projector_Calibrator::setInputCloud(Cloud& cloud){
// #define COMPUTE_NANS
input_cloud = cloud;
#ifdef COMPUTE_NANS
// count invalid points:
int input_nan = 0;
for (uint i=0; i<cloud.size(); ++i) {
pcl_Point p = cloud[i];
if (!(p.x == p.x)) input_nan++;
}
int output_nan = 0;
#endif
if (kinect_trafo_valid){
pcl::getTransformedPointCloud(input_cloud,kinect_trafo,cloud_moved);
#ifdef COMPUTE_NANS
for (uint i=0; i<cloud_moved.size(); ++i) {
pcl_Point p = cloud_moved[i];
if (!(p.x == p.x)) output_nan++;
}
#endif
}
#ifdef COMPUTE_NANS
ROS_INFO("NAN: input: %i, output: %i", input_nan, output_nan);
#endif
}
/**
* update each gaussian with the norm of the corresponding point
* points with frame_mask > 0 are not updated. This can be used to only update gaussians with
* background pixels. (use getForeground_* to compute such a mask)
*/
void PixelEnvironmentModel::update(const Cloud& cloud, cv::Mat* frame_mask, int step){
assert(int(cloud.width) == width_ && int(cloud.height) == height_);
timing_start("up_pixel");
update_cnt++;
if (mask_set){
// int cnt = cv::countNonZero(mask_);
// ROS_INFO("Update: mask has %i pixels",cnt);
}else{
ROS_INFO("No mask defined");
}
for (int y=0; y<height_; y += step)
for (int x=0; x<width_; x += step){
if (mask_set && mask_.at<uchar>(y,x) == 0) continue;
if (frame_mask && frame_mask->at<uchar>(y,x) > 0) continue;
double length = norm(cloud.at(x,y));
if (length > 0) // check for nans
gaussians[y][x].update(length);
}
timing_end("up_pixel");
}
/// lock all cells on which the marker points in the cloud are projected into
void Elevation_map::lockCells(const cv::Mat & mask, const Cloud& current){
if (locked.cols != mean.cols || locked.rows !=mean.rows || locked.type() == CV_8UC1){
locked = cv::Mat(mean.rows,mean.cols, CV_8UC1);
}
locked.setTo(0);
assert(mask.cols == int(current.width) && mask.rows == int(current.height));
for (int x=0; x<mask.cols; ++x)
for (int y=0; y<mask.rows; ++y){
if (mask.at<uchar>(y,x) == 0) continue;
cv::Point pos = grid_pos(current.at(x,y));
if (pos.x < 0) continue;
locked.at<uchar>(pos.y,pos.x) = 255;
}
locking_active = true;
// cv::namedWindow("blocked");
// cv::imshow("blocked",locked);
}
/**
* Initialization of grid from first measurement.
*
* @param cell_size length of grid cell in m
* @param cloud first cloud. size of grid is set so that all points have a minimum distance of 5cm to the border of the grid.
*/
void Elevation_map::init(float cell_size, const Cloud& cloud){
// pcl::getMinMax3d();
x_min_ = y_min_ = 1e6;
x_max_ = y_max_ = -1e6;
for (uint i=0; i<cloud.size(); ++i){
pcl_Point p = cloud[i];
if(p.x != p.x) continue;
x_min_ = min(x_min_, p.x);
y_min_ = min(y_min_, p.y);
x_max_ = max(x_max_, p.x);
y_max_ = max(y_max_, p.y);
}
// add small border:
x_min_ -= 0.05;
y_min_ -= 0.05;
x_max_ += 0.05;
y_max_ += 0.05;
init(cell_size, x_min_, x_max_,y_min_, y_max_);
}
MainMenu::MainMenu(CameraManager & cm) : Scene(&cm)
{
Cloud *cloud;
Balloon *balloon;
this->_buttons.push_back(new GameButton(glm::vec3(0, 50, 0), std::string("assets/textures/play.tga"), MainMenu::PLAY));
dynamic_cast<GameButton*>(this->_buttons.front())->setCurrent(true);
this->_buttons.push_back(new GameButton(glm::vec3(0, -150, 0), std::string("assets/textures/load.tga"), MainMenu::LOAD));
this->_buttons.push_back(new GameButton(glm::vec3(0, -350, 0), std::string("assets/textures/quit.tga"), MainMenu::QUIT));
// cloud = new Cloud(glm::vec3(-500, -300, 0), std::string("assets/textures/cloud.tga"));
// cloud->setScale(glm::vec3(250, 150, 0));
// this->addEntity(cloud);
cloud = new Cloud(glm::vec3(-700, 300, 0), std::string("assets/textures/cloud.tga"), 0.5f, 40.0f);
cloud->setScale(glm::vec3(400, 240, 0));
this->addEntity(cloud);
cloud = new Cloud(glm::vec3(500, 250, 0), std::string("assets/textures/cloud.tga"), 0.7f, 60.0f);
cloud->setScale(glm::vec3(150, 100, 0));
this->addEntity(cloud);
cloud = new Cloud(glm::vec3(400, -200, 0), std::string("assets/textures/cloud.tga"), 0.2f, 10.0f);
cloud->setScale(glm::vec3(170, 110, 0));
this->addEntity(cloud);
cloud = new Cloud(glm::vec3(300, -500, 0), std::string("assets/textures/cloud.tga"), 0.5f, 10.0f);
cloud->setScale(glm::vec3(230, 180, 0));
this->addEntity(cloud);
balloon = new Balloon(glm::vec3(-530, -90, 0), std::string("assets/textures/balloon.tga"), 0.5f, 10.0f);
balloon->setScale(glm::vec3(400, 469, 0));
this->addEntity(balloon);
// balloon = new Balloon(glm::vec3(570, 370, 0), std::string("assets/textures/up.tga"), 0.5f, 10.0f);
// balloon->setScale(glm::vec3(200, 269, 0));
// this->addEntity(balloon);
this->_eventHandler = new MenuEvent();
for (std::list<Pavement*>::iterator it = this->_buttons.begin(); it != this->_buttons.end(); it++)
{
(*it)->setScale(glm::vec3(300, 100, 100));
this->addEntity(*it);
}
this->_cursor = new GameButton(glm::vec3(-250, 50, 0), std::string("assets/textures/hat.tga"));
this->_cursor->setScale(glm::vec3(120, 120, 0));
this->_cursor->setCurrent(false);
this->addEntity(this->_cursor);
Pavement* background = new Pavement(glm::vec3(0, 0, 0), std::string("assets/textures/background.tga"));
background->setScale(glm::vec3(2500, 1300, 0));
this->addEntity(background);
}
void PixelEnvironmentModel::getCloudRepresentation(const Cloud& model, Cloud& result, float N){
result.clear();
uint8_t r = 255, g = 0, b = 0;
uint32_t rgb_red = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
r = 0; g = 255;
uint32_t rgb_green = ((uint32_t)r << 16 | (uint32_t)g << 8 | (uint32_t)b);
pcl_Point nap; nap.x = std::numeric_limits<float>::quiet_NaN();
for (int y = 0; y < height_; ++y)
for (int x = 0; x < width_; ++x){
if ((mask_set && mask_.at<uchar>(y,x) == 0) || !gaussians[y][x].initialized){
if (N<0)
result.push_back(nap);
continue;
}
float mean_dist = gaussians[y][x].mean;
// add point with same color but with norm = mean
pcl_Point p = model.at(x,y);
if (p.x != p.x) {
if (N<0) // keep cloud organized if no sigma-points are included
result.push_back(nap);
continue;
}
pcl_Point mean = setLength(p,mean_dist);
mean.rgb = p.rgb;
result.push_back(mean);
if (N > 0 && gaussians[y][x].initialized){
float sigma = gaussians[y][x].sigma();
pcl_Point near = setLength(p,mean_dist-N*sigma);
near.rgb = *reinterpret_cast<float*>(&rgb_green);
result.push_back(near);
pcl_Point far = setLength(p,mean_dist+N*sigma);
far.rgb = *reinterpret_cast<float*>(&rgb_red);
result.push_back(far);
}
}
if (N<0){
assert(int(result.size()) == width_*height_);
result.width = width_;
result.height = height_;
}
}
void applyBilateralFilter(const Cloud& in, double diameter, double sigmaDist, double sigmaPixel, Cloud& out){
cv::Mat dist(in.height, in.width, CV_32FC1);
out = in;
for (uint x=0; x<in.width; ++x)
for (uint y=0; y<in.height; ++y){
pcl_Point p = in.at(x,y);
if (p.x != p.x)
dist.at<float>(y,x) = 0;
else
dist.at<float>(y,x) = norm(p);
}
cv::Mat smoothed;
cv::bilateralFilter(dist, smoothed, diameter, sigmaDist, sigmaPixel);
float mean_change = 0;
int cnt= 0;
for (uint x=0; x<in.width; ++x)
for (uint y=0; y<in.height; ++y){
pcl_Point p = in.at(x,y);
float new_dist = smoothed.at<float>(y,x);
if (abs(new_dist) < 0.2) continue;
float old_dist = dist.at<float>(y,x);
mean_change += abs(new_dist-old_dist);
cnt++;
out.at(x,y) = setLength(p,new_dist);
}
mean_change /= cnt;
ROS_INFO("bilateral filter: mean change of %.2f cm", mean_change*100);
}
Cloud Background_substraction::removeBackground(const Cloud& current, float min_dist, float max_dist, std::vector<cv::Point2i>* valids){
Cloud result;
// assert(cloud.size() == reference.size());
// for (uint i=0; i<cloud.size(); ++i){
ROS_INFO("min: %f, max: %f", min_dist,max_dist);
if (min_dist == max_dist){
ROS_WARN("removeBackground called with min = %f and max = %f", min_dist, max_dist);
return result;
}
uint invalid = 0;
for (uint x=0; x<current.width; ++x)
for (uint y=0; y<current.height; ++y){
pcl_Point c = current.at(x,y);
if (c.x != c.x) { invalid++; continue;}
if (mask.at<uchar>(y,x) == 0) continue;
float d = norm(c);
float mean = means.at<float>(y,x);
// ROS_INFO("Norm: %f, mean: %f", d, mean);
if ( mean - min_dist > d && d > mean - max_dist){
result.push_back(c);
if (valids) valids->push_back(cv::Point2i(x,y));
}
}
// ROS_INFO("BG: %i invalid", invalid);
return result;
}
int main(int argc, char **argv) {
Cloud<Point3D>* cd = new Cloud<Point3D>();
cd->v_saveVisibleAttributes = true;
cd->loadFromFile("data/cloud.cl");
printf("Cloud loaded\n");
cd->saveToFile("data/cloud_copy.cl");
printf("Check manually that the files data/cloud.cl and data/cloud_copy.cl represent the same cloud\n");
Cloud<Point2D>* cd2 = new Cloud<Point2D>();
cd2->v_saveVisibleAttributes = true;
cd2->loadFromFile("data/cloud2D.cl");
printf("Cloud2D loaded\n");
cd2->saveToFile("data/cloud2D_copy.cl");
printf("Check manually that the files data/cloud2D.cl and data/cloud2D_copy.cl represent the same cloud\n");
}
Cloud Background_substraction::showBackground(const Cloud& cloud){
Cloud result; result = cloud;
for (uint x=0; x<cloud.width; ++x)
for (uint y=0; y<cloud.height; ++y){
float m = means.at<float>(y,x);
if (m > 0){
pcl_Point c = cloud.at(x,y);
if (c.x != c.x) continue;
setLength(c,m);
result.at(x,y) = c;
}
}
return result;
}
