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); } }
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; }
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); }
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 }
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++; } }
/** * 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_); }
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_; } }
pcl2::TypedMat<int> pcl2::findKNearestNeighbors (const Cloud & cloud, const MatF & query, size_t k) { // 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> indices (k); std::vector<float> dists (k); k = (size_t) tree.nearestKSearch (q, k, indices, dists); assert (k == indices.size ()); // Convert output EigenMat<int> output (k, 1); for (size_t i = 0; i < indices.size (); ++i) output (i, 0) = indices[i]; return (output); }
void Foam::ReactingMultiphaseParcel<ParcelType>::readFields ( Cloud<ParcelType>& cIn ) { if (!cIn.size()) { return; } ReactingMultiphaseCloud<ParcelType>& c = dynamic_cast<ReactingMultiphaseCloud<ParcelType>&>(cIn); ReactingParcel<ParcelType>::readFields(c); // Get names and sizes for each Y... const label idGas = c.composition().idGas(); const wordList& gasNames = c.composition().componentNames(idGas); const label idLiquid = c.composition().idLiquid(); const wordList& liquidNames = c.composition().componentNames(idLiquid); const label idSolid = c.composition().idSolid(); const wordList& solidNames = c.composition().componentNames(idSolid); const wordList& stateLabels = c.composition().stateLabels(); // Set storage for each Y... for each parcel forAllIter(typename Cloud<ParcelType>, c, iter) { ReactingMultiphaseParcel<ParcelType>& p = iter(); p.YGas_.setSize(gasNames.size(), 0.0); p.YLiquid_.setSize(liquidNames.size(), 0.0); p.YSolid_.setSize(solidNames.size(), 0.0); } // Populate YGas for each parcel forAll(gasNames, j) { IOField<scalar> YGas ( c.fieldIOobject ( "Y" + gasNames[j] + stateLabels[idGas], IOobject::MUST_READ ) ); label i = 0; forAllIter(typename Cloud<ParcelType>, c, iter) { ReactingMultiphaseParcel<ParcelType>& p = iter(); p.YGas_[j] = YGas[i++]/(p.Y()[GAS] + ROOTVSMALL); } }
bool Elevation_map::saveAsObj_2(std::string filename){ Cloud model = getModel(); std::ofstream off(filename.c_str()); if (!off.is_open()) return false; float size_factor = 1; int pos_in_file[model.size()]; int valid_pnt_cnt = 0; for (uint i=0; i<model.size(); ++i){ pcl_Point p = model[i]; off << "v " << size_factor*p.x << " " << size_factor*p.y << " " << size_factor*p.z << "\r\n"; pos_in_file[i] = valid_pnt_cnt++; } int w = model.width; for (uint x=0; x<model.width-1; ++x){ for (uint y=0; y<model.height-1; ++y){ int a_idx = x+y*w; int b_idx = (x+1)+y*w; int c_idx = x+(y+1)*w; int d_idx = (x+1)+(y+1)*w; int a = pos_in_file[a_idx]+1; // retrieve position in .obj-file int b = pos_in_file[b_idx]+1; int c = pos_in_file[c_idx]+1; int d = pos_in_file[d_idx]+1; off << "f " << a << " " << b << " " << c << "\r\n"; off << "f " << b << " " << d << " " << c << "\r\n"; } } return true; }
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]]; }
void Foam::Cloud<ParticleType>::checkFieldIOobject ( const Cloud<ParticleType>& c, const IOField<DataType>& data ) const { if (data.size() != c.size()) { FatalErrorIn ( "void Cloud<ParticleType>::checkFieldIOobject" "(Cloud<ParticleType>, IOField<DataType>)" ) << "Size of " << data.name() << " field does not match the number of particles" << abort(FatalError); } }
// untested Cloud Background_substraction::applyMask(Cloud& current){ if (uint(mask.cols) != current.width){ ROS_WARN("no background computed!"); return Cloud(); } Cloud result; result.reserve(current.size()); for (uint x=0; x<current.width; ++x) for (uint y=0; y<current.height; ++y){ if (mask.at<uchar>(y,x) == 255) result.push_back(current.at(x,y)); } return result; }
void Foam::ThermoParcel<ParcelType>::writeFields(const Cloud<ParcelType>& c) { KinematicParcel<ParcelType>::writeFields(c); label np = c.size(); IOField<scalar> T(c.fieldIOobject("T", IOobject::NO_READ), np); IOField<scalar> cp(c.fieldIOobject("cp", IOobject::NO_READ), np); label i = 0; forAllConstIter(typename Cloud<ParcelType>, c, iter) { const ThermoParcel<ParcelType>& p = iter(); T[i] = p.T_; cp[i] = p.cp_; i++; } T.write(); cp.write(); }
/// transfers the information (vertices, normals, triangles) to the trianglemesh void Surface_Modeler::copyToMesh(rms::VFTriangleMesh& mesh){ Cloud model = getModel(); Cloud_n with_normals; computeNormals(model, with_normals); Wml::Vector3f e_z = Wml::Vector3f::UNIT_Z; for (uint i=0; i<model.size(); ++i){ pcl_Point p = model[i]; pcl_Normal n = with_normals[i]; mesh.AppendVertex(Wml::Vector3f(p.x,p.y,p.z)); // hack! compute normal for every point! if (n.normal_x == n.normal_x) mesh.SetNormal(i,Wml::Vector3f(n.normal_x, n.normal_y,n.normal_z)); else mesh.SetNormal(i,e_z); } int w = model.width; // add triangles: for (uint x=0; x<model.width-1; ++x) for (uint y=0; y<model.height-1; ++y){ // left upper triangle int a = x+y*w; int b = (x+1)+y*w; int c = x+(y+1)*w; int d = (x+1)+(y+1)*w; mesh.AppendTriangle(a,b,c); mesh.AppendTriangle(b,d,c); } ROS_INFO("Mesh has %i vertices and %i triangles", mesh.GetVertexCount(), mesh.GetTriangleCount()); }
/// Save model in .obj-Format with normals bool Elevation_map::saveAsObj(std::string filename, bool ignore_triangles_without_normals){ Cloud model = getModel(); Cloud_n with_normals; // computeNormals(model, with_normals); // ROS_INFO("Model has %zu points, computed normals: %zu", model.size(), with_normals.size()); std::ofstream off(filename.c_str()); if (!off.is_open()) return false; int normal_pos_in_file[model.size()]; int valid_pnt_cnt = 0; // bool has_normal[model.size()]; bool write_normals = true; // write points for (uint i=0; i<model.size(); ++i){ pcl_Point p = model[i]; pcl_Normal n = with_normals[i]; off << "v " << p.x << " " << p.y << " " << p.z << "\r\n"; // evil hack! compute normals also for border of if (n.normal_x != n.normal_x){ n.normal_x = n.normal_y = 0; n.normal_z = 1; } //if (write_normals) off << "vn " << n.normal_x << " " << n.normal_y << " " << n.normal_z << "\r\n"; normal_pos_in_file[i] = valid_pnt_cnt++; // if (n.normal_x == n.normal_x){ // //if (write_normals) // off << "vn " << n.normal_x << " " << n.normal_y << " " << n.normal_z << "\r\n"; // normal_pos_in_file[i] = valid_pnt_cnt++; // }else{ // normal_pos_in_file[i] = -1; // } } // std::set<int> invalid_normals; // // // write normals // for (uint i=0; i<model.size(); ++i){ // // // if (n.normal_x != n.normal_x) // invalid_normals.insert(i+1); // .obj uses 1-based indices // // // invalid normal is also included into the file to keep the numbering consistent // } // model.at(u,v); v * this->width + u) // ROS_INFO("modelsize: %i %i", model.width, model.height); int w = model.width; // ROS_INFO("Writing faces"); // write faces /** * TODO: * * invalid normales are in most cases the outer points that don't have enough neighbours. * */ // bool only_faces = false; for (uint x=0; x<model.width-1; ++x){ for (uint y=0; y<model.height-1; ++y){ // left upper triangle int a_idx = x+y*w; int b_idx = (x+1)+y*w; int c_idx = x+(y+1)*w; int d_idx = (x+1)+(y+1)*w; // if (only_faces){ // off << "f " << a << " " << b << " " << c << "\r\n"; // off << "f " << b << " " << d << " " << c << "\r\n"; // // continue; // } // bool a_normal_valid = (invalid_normals.find(a) == invalid_normals.end()); // bool b_normal_valid = (invalid_normals.find(b) == invalid_normals.end()); // bool c_normal_valid = (invalid_normals.find(c) == invalid_normals.end()); // bool d_normal_valid = (invalid_normals.find(d) == invalid_normals.end()); int a = normal_pos_in_file[a_idx]; // compute position in .obj-file int b = normal_pos_in_file[b_idx]; int c = normal_pos_in_file[c_idx]; int d = normal_pos_in_file[d_idx]; bool a_normal_valid = a >= 0; // check if point was added to the file (aka normal was valid) bool b_normal_valid = b >= 0; bool c_normal_valid = c >= 0; bool d_normal_valid = d >= 0; a+=1; b+=1; c+=1; d+=1; // oppa matlab style a_idx+=1;b_idx+=1;c_idx+=1;d_idx+=1; if (write_normals && a_normal_valid && b_normal_valid && c_normal_valid){ // first index for point, second index for normal off << "f " << a_idx << "//" << a << " " << b_idx << "//" << b << " " << c_idx << "//" << c << "\r\n"; }else{ //if (!ignore_triangles_without_normals) off << "f " << a_idx << " " << b_idx << " " << c_idx << "\r\n"; } // right lower triangle if (write_normals && b_normal_valid && c_normal_valid && d_normal_valid) off << "f " << b_idx << "//" << b << " " << d_idx << "//" << d << " " << c_idx << "//" << c << "\r\n"; else{ //if (!ignore_triangles_without_normals) off << "f " << b_idx << " " << d_idx << " " << c_idx << "\r\n"; } } } off.close(); ROS_INFO("Closing file"); return true; }
void MainWindow::show_model_openGL(){ qApp->processEvents(); timing_start("open_gl"); if (!qnode.elevation_map.modelComputed()){ ROS_INFO("Model is not computed"); return; } // ros::Time now_getmodel = ros::Time::now(); // timing_start("get model"); Cloud model; qnode.elevation_map.getModel(model); // timing_end("get model"); // Cloud model = qnode.pixel_modeler.getModel(qnode.modeler.min_x(),qnode.modeler.min_y(), // qnode.modeler.cell_size(),qnode.modeler.cell_size()); // Cloud model; // qnode.pixel_modeler.getCloudRepresentation(qnode.current_cloud,model,-1); // pcl::getTransformedPointCloud(model,qnode.calibrator.kinect_trafo,model); // ROS_INFO("getModel: %f ms", (ros::Time::now()-now_getmodel).toSec()*1000); if (model.size() == 0){ ROS_INFO("Model is not computed (and empty)"); return; } // if (!(qnode.openGL_visualizationActive || qnode.water_simulation_active)){ // return; // } // TODO: create only once after model is initialized cv::Mat color(model.height, model.width, CV_8UC3); color.setTo(0); // if (qnode.openGL_visualizationActive){ // TODO: don't copy image cv::Mat height = qnode.elevation_map.getHeightImage(); // cv::Mat height; // qnode.pixel_modeler.getMeans(height); heightVisualization(color, height, 0,500, qnode.color_range,NULL); // } // cv::imwrite("height.png", color); if (qnode.water_simulation_active){ // white surrounding if no height lines are visualized // if (!qnode.openGL_visualizationActive) // color.setTo(255); // ROS_INFO("water cols: %i, model: %i", qnode.water.cols, model.width); if (qnode.water.cols == int(model.width)){ waterVisualization(color, qnode.water, 0.005,0.02,NULL); // cv::Mat alpha; // todo: don't generate each time // waterVisualizationAlpha(color, alpha,qnode.water, 0.005,0.02,NULL); // cv::imwrite("water.png", color); // cv::imwrite("alpha.png", alpha); }else{ ROS_ERROR("qnode.water.cols == int(model.width)"); } } Cloud colored = transferColorToMesh(color, model, NULL); #ifdef DO_TIMING ROS_INFO("colorizeCloud: %f ms", (ros::Time::now()-now_color).toSec()*1000.0); #endif ros::Time now_mesh = ros::Time::now(); float max_edge_length = 0.05; // max edge length of drawn triangle in m gl_viewer->cloud = colored; qnode.mesh_visualizer.createMesh(gl_viewer->cloud,gl_viewer->mesh, max_edge_length); #ifdef DO_TIMING ROS_INFO("createMesh: %f ms", (ros::Time::now()-now_mesh).toSec()*1000.0); #endif gl_viewer->proj_matrix = qnode.calibrator.proj_Matrix(); assert(gl_viewer->proj_matrix.type() == CV_64FC1); // new stuff: gl_viewer->world2Projector = qnode.calibrator.cal_cv_with_dist.proj_trafo; gl_viewer->cam_internals = qnode.calibrator.cal_cv_with_dist.camera_matrix; // cv::Mat P = qnode.calibrator.proj_matrix_cv; // cout << "Projection Matrix openCV " << endl << P << endl; // cout << "cv internal " << endl << qnode.calibrator.camera_matrix_CV << endl; // cout << "cv trafo " << endl << qnode.calibrator.proj_trafo_CV << endl; // cv::Mat p = qnode.calibrator.camera_matrix_CV*qnode.calibrator.proj_trafo_CV; // p /= p.at<double>(3,3); // cout << "product: " << p << endl; // cv::Point2f px = applyPerspectiveTrafo(cv::Point3f(0,0,0),P); // ROS_INFO("0,0,0 was projected to %f %f", px.x, px.y); if (qnode.show_height_lines){ timing_start("height_lines"); std::vector<Line_collection> height_lines; float min_height = qnode.elevation_map.getMinHeight(); float max_height = qnode.elevation_map.getMaxHeight(); float dist = qnode.height_line_distance; // get first and last visible height line: float min_ = ceil(min_height/dist)*dist; float max_ = floor(max_height/dist)*dist; qnode.mesh_visualizer.findHeightLines(gl_viewer->mesh, gl_viewer->cloud,height_lines, min_,max_,dist); gl_viewer->set_height_lines(height_lines); timing_end("height_lines"); }else{ std::vector<Line_collection> height_lines; gl_viewer->set_height_lines(height_lines); } #ifdef DO_TIMING ROS_INFO("Showing Model with openGL (total): %f ms", (ros::Time::now()-start_show_openGL).toSec()*1000); #endif gl_viewer->initMapSize(qnode.elevation_map.min_x(),qnode.elevation_map.min_y(),qnode.elevation_map.getWidth(), qnode.elevation_map.getHeight()); // // draws mesh and heigt_lines (if set) timing_start("Rendering"); // gl_viewer->withDistortion(true); // render with distortion gl_viewer->show_fullscreenimage = false; gl_viewer->updateGL(); timing_end("Rendering"); // Optional: Also render ortographic projection (geographical Map) if (show_map_image){ QPixmap map_image = gl_viewer->getMapImage(ui.lb_img_2->width(),ui.lb_img_2->height()); ui.lb_img_2->setPixmap(map_image); ui.lb_img_2->repaint(); } //#ifdef DO_TIMING timing_end("open_gl"); //#endif }
void MultiView::reconstructNext(long frameId1, const vector<Point2f> &points1, long frameId2, const vector<Point2f> &points2) { // cout << "frameId1: " << frameId1 << endl; map<pair<int, int>, int> &a = cloud.lookupIdxBy2D[frameId1]; vector<Point3f> points3d; vector<Point2f> imgpoints; // collecting 2D matching points for 3D coordinates for (int i = 0; i < points1.size(); i++) { Point2i pt(points1[i]); map<pair<int, int>, int>::iterator it(a.find(makePair(pt))); if (it != a.end()) { // we have a cloudpoint for points1[i], let's save the 2d point imgpoints.push_back(points2[i]); // and the cloudpoint points3d.push_back(cloud.points[it->second].pt); // cout << pt << ": " << a.count(makePair(pt)) << endl; } } //cout.flush(); cv::Matx34d P1 = Pmats[frameId1]; cv::Mat_<double> t = (cv::Mat_<double>(1, 3) << P1(0, 3), P1(1, 3), P1(2, 3)); cv::Mat_<double> R = (cv::Mat_<double>(3, 3) << P1(0, 0), P1(0, 1), P1(0, 2), P1(1, 0), P1(1, 1), P1(1, 2), P1(2, 0), P1(2, 1), P1(2, 2)); cv::Mat_<double> rvec(1, 3); Rodrigues(R, rvec); vector<Point2f> reprojected; FindPoseEstimation(cam, rvec, t, R, points3d, imgpoints, reprojected); Matx34d P2 = Matx34d(R(0, 0), R(0, 1), R(0, 2), t(0), R(1, 0), R(1, 1), R(1, 2), t(1), R(2, 0), R(2, 1), R(2, 2), t(2)); this->addP(frameId2, P2); cout << "===========" << endl; cout << Pmats[frameId1] << endl; cout << Pmats[frameId2] << endl; // triangulateIteratively new points Cloud pc; vector<Point> correspond; double reproj_error = TriangulatePoints(frameId1, points1, points2, cam->cameraMatrix, cam->Kinv, cam->distCoeffs, Pmats[frameId1], Pmats[frameId2], pc, correspond); // std::cout << "triangulation reproj error " << reproj_error << std::endl; vector<uchar> trig_status; if (!TestTriangulation(pc, Pmats[frameId1], trig_status) || !TestTriangulation(pc, Pmats[frameId2], trig_status)) { cerr << "Triangulation did not succeed" << endl; return; } vector<CloudPoint> new_points; for (unsigned int i = 0; i < pc.size(); i++) { // FIXME -- what this should be??? 80% percentile??? // SAME AS IN OF RECONSTRUCTION,, move this out?? if (pc.points[i].reprojection_error < 10.0 && trig_status[i]) { cloud.insert(pc.points[i], frameId1, points1[i], frameId2, points2[i]); new_points.push_back(pc.points[i]); } } // writeCloudPoints("cloud" + std::to_string(frame++) + ".ply", new_points); //cloud.points); // writeCloudPoints("cloud_full.ply", cloud.points); }
/** * * @param cloud new measurement. For each bin, the average height (h_new) of the new points falling into this bin is calculated. If the * old height was h_old, it is updated to h = (1-weight)h_old+weight*h_new. * * @todo (speedup) combine with getFGMask * @return: */ bool Elevation_map::updateHeight(const Cloud& cloud, float min_diff_m){ timing_start("update_height"); bool min_diff_active = min_diff_m > 0; update_count++; hits.setTo(0); height_sum.setTo(0); int step = 2; // increase size of gaussian blur accordingly // cv::Mat updated(480,640,CV_8UC1); // updated.setTo(0); if (locking_active){ ROS_INFO("locked: %i %i, hits: %i %i", locked.rows,locked.cols, hits.rows, hits.cols); assert(locked.cols == hits.cols); assert(locked.rows == hits.rows); assert(locked.type() == CV_8UC1); } // if (locking_active){ // ROS_INFO("Locked: %i %i, cells: %i %i", locked.cols, locked.rows, cell_cnt_x, cell_cnt_y); // } for (uint i=0; i<cloud.size(); i += step){ pcl_Point p = cloud[i]; cv::Point pos = grid_pos(p); if (pos.x < 0) continue; assert(0 <= pos.y && 0 <= pos.x && pos.y < cell_cnt_y && pos.x < cell_cnt_x); if (locking_active){ // if (!(pos.y < locked.rows && pos.x < locked.cols)){ // ROS_INFO("pos: %i %i, size: %i %i", pos.x,pos.y,locked.cols, locked.rows); // } if (locked.at<uchar>(pos.y,pos.x) > 0){ continue; } } height_sum.at<float>(pos.y,pos.x) += p.z; hits.at<float>(pos.y,pos.x)++; // updated.at<uchar>(pos.y,pos.x) = 255; } // cv::namedWindow("updated"); // cv::imshow("updated",updated); int dyn_pixel_cnt = 0; for (int x=0; x<cell_cnt_x; ++x) for(int y=0; y<cell_cnt_y; ++y){ float hit_cnt = hits.at<float>(y,x); if (hit_cnt > 0){ float height = height_sum.at<float>(y,x)/hit_cnt; float old = mean.at<float>(y,x); if (first_frame || (old != old)){ z_min = min(height*1.0, z_min); z_max = max(height*1.0, z_max); mean.at<float>(y,x) = height; } else{ // ignore too large updates (they correspond to the hand moving over the surface) if (!min_diff_active || abs(height-old) < min_diff_m){ float h_new = (1-weight)*old+weight*height; z_min = min(h_new*1.0, z_min); z_max = max(h_new*1.0, z_max); mean.at<float>(y,x) = h_new; }else{ dyn_pixel_cnt++; } } } } // ROS_INFO("Found %i dynamic pixels", dyn_pixel_cnt); first_frame = false; model_computed = true; // new model was computed model_3d_valid = false;// therefore, the old 3d-model is not longer valid and will be recreated on demand //small blur to smooth heightfield cv::Mat mean_old; if (locking_active) mean.copyTo(mean_old); int blur_size = 3; cv::GaussianBlur(mean, mean, cv::Size(blur_size,blur_size),2,2); if (locking_active){ cv::Mat locked_dilated; cv::dilate(locked, locked_dilated, cv::Mat(), cv::Point(-1,-1), blur_size); mean_old.copyTo(mean,locked_dilated); // reset mean at locked cells } timing_end("update_height"); // due to measurement errors, some pixels have a high error. If a hand is visible in the image, // more than 500 Pixels are counted. return dyn_pixel_cnt > 10; }
Foam::lagrangianFieldDecomposer::lagrangianFieldDecomposer ( const polyMesh& mesh, const polyMesh& procMesh, const labelList& faceProcAddressing, const labelList& cellProcAddressing, const word& cloudName, const Cloud<indexedParticle>& lagrangianPositions, const List<SLList<indexedParticle*>*>& cellParticles ) : procMesh_(procMesh), positions_(procMesh, cloudName, false), particleIndices_(lagrangianPositions.size()) { label pi = 0; // faceProcAddressing not required currently // labelList decodedProcFaceAddressing(faceProcAddressing.size()); // forAll(faceProcAddressing, i) // { // decodedProcFaceAddressing[i] = mag(faceProcAddressing[i]) - 1; // } forAll(cellProcAddressing, procCelli) { label celli = cellProcAddressing[procCelli]; if (cellParticles[celli]) { SLList<indexedParticle*>& particlePtrs = *cellParticles[celli]; forAllConstIter(SLList<indexedParticle*>, particlePtrs, iter) { const indexedParticle& ppi = *iter(); particleIndices_[pi++] = ppi.index(); // label mappedTetFace = findIndex // ( // decodedProcFaceAddressing, // ppi.tetFace() // ); // if (mappedTetFace == -1) // { // FatalErrorInFunction // << "Face lookup failure." << nl // << abort(FatalError); // } positions_.append ( new passiveParticle ( procMesh, ppi.position(), procCelli, false ) ); } } }