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
)
);
}
}
}
