void Layouter::laplacianEigMap( const MatrixXd& laplacian, const VectorXf& radiusVec, const VectorXi& hashID, MatrixXf& finalPos2D, float& finalRadius, float sparseFactor /*= 1.f*/ ) { if (laplacian.rows() <= 0 || laplacian.cols() <= 0 || radiusVec.size() <= 0) { finalPos2D = MatrixXf::Zero(1,2); finalRadius = 0; return; } if (laplacian.rows() == 1 && laplacian.cols() == 1 && radiusVec.size() == 1) { finalPos2D = MatrixXf::Zero(1,2); finalRadius = radiusVec[0]; return; } MatrixXd pos2D, finalPosd; LaplacianSolver::compute(laplacian, pos2D); VectorXd minPos, maxPos; MDSPostProcesser m_postProcessor(500, 1.0f, 1.0, 1.02, radiusVec.minCoeff()); m_postProcessor.setSparseFactor(sparseFactor); m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID); m_postProcessor.compute(); m_postProcessor.getFinalPos(finalPosd); m_postProcessor.getFinalBoundingRect(minPos, maxPos); finalPos2D = finalPosd.cast<float>(); // finalPos2D = pos2D.cast<float>(); // minPos = pos2D.colwise().minCoeff(); // maxPos = pos2D.colwise().maxCoeff(); // VectorXd size = maxPos - minPos; finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f; }
void Layouter::mds(const MatrixXd& distMat, const VectorXf& radiusVec, const VectorXi& hashID, MatrixXf& finalPos2D, float& finalRadius, float sparseFactor, float paddingRatio) { if (distMat.rows() <= 0 || distMat.cols() <= 0 || radiusVec.size() <= 0) { finalPos2D = MatrixXf::Zero(1,2); finalRadius = 0; return; } if (distMat.rows() == 1 && distMat.cols() == 1 && radiusVec.size() == 1) { finalPos2D = MatrixXf::Zero(1,2); finalRadius = radiusVec[0]; return; } MatrixXd pos2D, finalPosd; ClassicalMDSSolver::compute(distMat, pos2D); VectorXd minPos, maxPos; float minPadding = radiusVec.minCoeff(); MDSPostProcesser m_postProcessor(5000, sparseFactor, 1.0, paddingRatio, minPadding); m_postProcessor.set2DPos(pos2D, radiusVec.cast<double>(), &hashID); m_postProcessor.compute(); m_postProcessor.getFinalPos(finalPosd); m_postProcessor.getFinalBoundingRect(minPos, maxPos); finalPos2D = finalPosd.cast<float>(); // finalPos2D = pos2D.cast<float>(); // minPos = pos2D.colwise().minCoeff(); // maxPos = pos2D.colwise().maxCoeff(); // VectorXd size = maxPos - minPos; finalRadius = m_postProcessor.getFinalRadius();// size.norm() * 0.5;//(size[0] > size[1] ? size[0] : size[1]) * 0.5f; }
void wavePlotter::autoScaleRange(VectorXf &data) { lowRange = data.minCoeff();//Utils::ofMin(&data(0), DATA_SIZE); highRange = data.maxCoeff();//Utils::ofMax(&data(0), DATA_SIZE); }
void D3DCloudProjector::projectCloud(int id, const sensor_msgs::PointCloud& data, const std::vector<int>& interest_region_indices) { MatrixXf& oriented = orienter_->oriented_clouds_[id]; // -- Get a copy of the projected points. MatrixXf projected(oriented.rows(), 2); int c=0; for(int i=0; i<3; ++i) { if(i == axis_of_projection_) continue; projected.col(c) = oriented.col(i); ++c; } // -- Transform into pixel units. projected is currently in meters, centered at 0. //projected *= pixels_per_meter_; for(int i=0; i<projected.rows(); ++i) { projected(i, 0) *= pixels_per_meter_; projected(i, 1) *= pixels_per_meter_; } // -- Find min and max of u and v. TODO: noise sensitivity? // u is the col number in the image plane, v is the row number. float min_v = FLT_MAX; float min_u = FLT_MAX; float max_v = -FLT_MAX; float max_u = -FLT_MAX; for(int i=0; i<projected.rows(); ++i) { float u = projected(i, 0); float v = projected(i, 1); if(u < min_u) min_u = u; if(u > max_u) max_u = u; if(v < min_v) min_v = v; if(v > max_v) max_v = v; } // -- Translate to coordinate system where (0,0) is the upper right of the image. for(int i=0; i<projected.rows(); ++i) { projected(i, 0) -= min_u; projected(i, 1) = max_v - projected(i, 1); } // -- Get the max depth. float max_depth = -FLT_MAX; float min_depth = FLT_MAX; for(int i=0; i<oriented.rows(); ++i) { if(oriented(i, axis_of_projection_) > max_depth) max_depth = oriented(i, axis_of_projection_); if(oriented(i, axis_of_projection_) < min_depth) min_depth = oriented(i, axis_of_projection_); } // -- Compute the normalized depths. Depths are between 0 and 1, with 1 meaning closest and 0 meaning furthest. VectorXf depths = oriented.col(axis_of_projection_); if(axis_of_projection_ == 1) depths = -depths; depths = depths.cwise() - depths.minCoeff(); depths = depths / depths.maxCoeff(); // -- Fill the IplImages. assert(sizeof(float) == 4); CvSize size = cvSize(ceil(max_u - min_u), ceil(max_v - min_v)); IplImage* acc = cvCreateImage(size, IPL_DEPTH_32F, 1); IplImage* intensity = cvCreateImage(size, IPL_DEPTH_32F, 1); IplImage* depth = cvCreateImage(size, IPL_DEPTH_32F, 1); cvSetZero(acc); cvSetZero(depth); cvSetZero(intensity); assert(projected.rows() == (int)interest_region_indices.size()); for(int i=0; i<projected.rows(); ++i) { int row = floor(projected(i, 1)); int col = floor(projected(i, 0)); // Update accumulator. assert(interest_region_indices[i] < (int)data.channels[0].values.size() && (int)interest_region_indices[i] >= 0); ((float*)(acc->imageData + row * acc->widthStep))[col]++; // Add to intensity values. float val = (float)data.channels[0].values[interest_region_indices[i]] / 255.0 * (3.0 / 4.0) + 0.25; assert(val <= 1.0 && val >= 0.0); ((float*)(intensity->imageData + row * intensity->widthStep))[col] += val; // Add to depth values. ((float*)(depth->imageData + row * depth->widthStep))[col] += depths(i); // } // -- Normalize by the number of points falling in each pixel. for(int v=0; v<acc->height; ++v) { float* intensity_ptr = (float*)(intensity->imageData + v * intensity->widthStep); float* depth_ptr = (float*)(depth->imageData + v * depth->widthStep); float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep); for(int u=0; u<acc->width; ++u) { if(*acc_ptr == 0) { *intensity_ptr = 0; *depth_ptr = 0; } else { *intensity_ptr = *intensity_ptr / *acc_ptr; *depth_ptr = *depth_ptr / *acc_ptr; } intensity_ptr++; depth_ptr++; acc_ptr++; } } // -- Store images. depth_projections_.push_back(depth); intensity_projections_.push_back(intensity); // -- Debugging. if(debug_) { float scale = 10; IplImage* intensity_big = cvCreateImage(cvSize(((float)intensity->width)*scale, ((float)intensity->height)*scale), intensity->depth, intensity->nChannels); cvResize(intensity, intensity_big, CV_INTER_AREA); IplImage* depth_big = cvCreateImage(cvSize(((float)depth->width)*scale, ((float)depth->height)*scale), depth->depth, depth->nChannels); cvResize(depth, depth_big, CV_INTER_AREA); CVSHOW("Intensity Image", intensity_big); CVSHOW("Depth Image", depth_big); cvWaitKey(0); cvDestroyWindow("Intensity Image"); cvDestroyWindow("Depth Image"); } // -- Clean up. cvReleaseImage(&acc); }
void constrain_marginals_bp ( MatrixXf & joint, const VectorXf & prior_dom, const VectorXf & prior_cod, VectorXf & temp_dom, VectorXf & temp_cod, float tol, size_t max_steps, bool logging) { // Enforce simultaineous constraints on a joint PMF // // /\x. sum y. J(y,x) = p(x) // /\y. sum x. J(y,x) = q(y) ASSERT_EQ(prior_dom.size(), joint.cols()); ASSERT_EQ(prior_cod.size(), joint.rows()); ASSERT_LT(0, prior_dom.minCoeff()); ASSERT_LT(0, prior_cod.minCoeff()); if (logging) LOG(" constraining marginals via full BP"); const size_t X = joint.cols(); const size_t Y = joint.rows(); const Vector<float> p = as_vector(prior_dom); const Vector<float> q = as_vector(prior_cod); Vector<float> J = as_vector(joint); Vector<float> sum_y_J = as_vector(temp_dom); Vector<float> sum_x_J = as_vector(temp_cod); float stepsize = 0; size_t steps = 0; while (steps < max_steps) { ++steps; if (logging) cout << " step " << steps << "/" << max_steps << flush; stepsize = 0; // constrain sum y. J(y,x) = 1 first, // in case joint is initalized with conditional for (size_t x = 0; x < X; ++x) { Vector<float> J_x = J.block(Y, x); sum_y_J[x] = sum(J_x); } ASSERT_LT(0, min(sum_y_J)); // XXX error here imax(stepsize, sqrtf(max_dist_squared(sum_y_J, p))); idiv_store_rhs(p, sum_y_J); for (size_t x = 0; x < X; ++x) { Vector<float> J_x = J.block(Y, x); J_x *= sum_y_J[x]; } sum_x_J.zero(); for (size_t x = 0; x < X; ++x) { Vector<float> J_x = J.block(Y, x); sum_x_J += J_x; } ASSERT_LT(0, min(sum_x_J)); imax(stepsize, sqrtf(max_dist_squared(sum_x_J, q))); idiv_store_rhs(q, sum_x_J); for (size_t x = 0; x < X; ++x) { Vector<float> J_x = J.block(Y, x); J_x *= sum_x_J; } if (logging) LOG(", stepsize = " << stepsize); if (stepsize < tol) break; } }
void CloudProjector::_compute() { assert(orienter_); assert(orienter_->getOutputCloud()); assert(!depth_projection_); assert(!intensity_projection_); MatrixXf& oriented = *orienter_->getOutputCloud(); VectorXf& intensities = *orienter_->getOutputIntensity(); // -- Get a copy of the projected points. MatrixXf projected(oriented.rows(), 2); int c=0; for(int i=0; i<3; ++i) { if(i == axis_of_projection_) continue; projected.col(c) = oriented.col(i); ++c; } // -- Transform into pixel units. projected is currently in meters, centered at 0. //projected *= pixels_per_meter_; for(int i=0; i<projected.rows(); ++i) { projected(i, 0) *= pixels_per_meter_; projected(i, 1) *= pixels_per_meter_; } // -- Find min and max of u and v. TODO: noise sensitivity? // u is the col number in the image plane, v is the row number. float min_v = FLT_MAX; float min_u = FLT_MAX; float max_v = -FLT_MAX; float max_u = -FLT_MAX; for(int i=0; i<projected.rows(); ++i) { float u = projected(i, 0); float v = projected(i, 1); if(u < min_u) min_u = u; if(u > max_u) max_u = u; if(v < min_v) min_v = v; if(v > max_v) max_v = v; } // -- Translate to coordinate system where (0,0) is the upper right of the image. for(int i=0; i<projected.rows(); ++i) { projected(i, 0) -= min_u; projected(i, 1) = max_v - projected(i, 1); } // -- Get the max depth. float max_depth = -FLT_MAX; float min_depth = FLT_MAX; for(int i=0; i<oriented.rows(); ++i) { if(oriented(i, axis_of_projection_) > max_depth) max_depth = oriented(i, axis_of_projection_); if(oriented(i, axis_of_projection_) < min_depth) min_depth = oriented(i, axis_of_projection_); } // -- Compute the normalized depths. Depths are between 0 and 1, with 1 meaning closest and 0 meaning furthest. VectorXf depths = oriented.col(axis_of_projection_); if(axis_of_projection_ == 1) depths = -depths; depths = (depths.array() - depths.minCoeff()).matrix(); depths = depths / depths.maxCoeff(); // -- Fill the IplImages. assert(sizeof(float) == 4); CvSize size = cvSize(ceil(max_u - min_u) + 1, ceil(max_v - min_v) + 1); float pad_width = 0; if(min_width_ > 0 && size.width < min_width_) { pad_width = (float)(min_width_ - size.width) / 2.; size.width = min_width_; } float pad_height = 0; if(min_height_ > 0 && size.height < min_height_) { pad_height = (float)(min_height_ - size.height) / 2.; size.height = min_height_; } IplImage* acc = cvCreateImage(size, IPL_DEPTH_32F, 1); intensity_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1); depth_projection_ = cvCreateImage(size, IPL_DEPTH_32F, 1); cvSetZero(acc); cvSetZero(depth_projection_); cvSetZero(intensity_projection_); assert(intensities.rows() == projected.rows()); for(int i=0; i<projected.rows(); ++i) { int row = floor(projected(i, 1) + pad_height); int col = floor(projected(i, 0) + pad_width); assert(row < size.height && row >= 0 && col < size.width && col >= 0); // Update accumulator. ((float*)(acc->imageData + row * acc->widthStep))[col]++; // Update intensity values. float val = intensities(i) / 255.0 * (3.0 / 4.0) + 0.25; assert(val <= 1.0 && val >= 0.0); ((float*)(intensity_projection_->imageData + row * intensity_projection_->widthStep))[col] += val; // Update depth values. ((float*)(depth_projection_->imageData + row * depth_projection_->widthStep))[col] += depths(i); // } // -- Normalize by the number of points falling in each pixel. for(int v=0; v<acc->height; ++v) { float* intensity_ptr = (float*)(intensity_projection_->imageData + v * intensity_projection_->widthStep); float* depth_ptr = (float*)(depth_projection_->imageData + v * depth_projection_->widthStep); float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep); for(int u=0; u<acc->width; ++u) { if(*acc_ptr == 0) { *intensity_ptr = 0; *depth_ptr = 0; } else { *intensity_ptr = *intensity_ptr / *acc_ptr; *depth_ptr = *depth_ptr / *acc_ptr; } intensity_ptr++; depth_ptr++; acc_ptr++; } } // -- Blur the images. TODO: depth too? cvSmooth(intensity_projection_, intensity_projection_, CV_GAUSSIAN, smoothing_, smoothing_); // -- Clean up. cvReleaseImage(&acc); }