template <typename PointInT, typename PointNT, typename PointOutT> void
pcl::FPFHEstimation<PointInT, PointNT, PointOutT>::computeSPFHSignatures (std::vector<int> &spfh_hist_lookup,
Eigen::MatrixXf &hist_f1, Eigen::MatrixXf &hist_f2, Eigen::MatrixXf &hist_f3)
{
// Allocate enough space to hold the NN search results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
std::set<int> spfh_indices;
spfh_hist_lookup.resize (surface_->points.size ());
// Build a list of (unique) indices for which we will need to compute SPFH signatures
// (We need an SPFH signature for every point that is a neighbor of any point in input_[indices_])
if (surface_ != input_ ||
indices_->size () != surface_->points.size ())
{
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
int p_idx = (*indices_)[idx];
if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
spfh_indices.insert (nn_indices.begin (), nn_indices.end ());
}
}
else
{
// Special case: When a feature must be computed at every point, there is no need for a neighborhood search
for (size_t idx = 0; idx < indices_->size (); ++idx)
spfh_indices.insert (static_cast<int> (idx));
}
// Initialize the arrays that will store the SPFH signatures
size_t data_size = spfh_indices.size ();
hist_f1.setZero (data_size, nr_bins_f1_);
hist_f2.setZero (data_size, nr_bins_f2_);
hist_f3.setZero (data_size, nr_bins_f3_);
// Compute SPFH signatures for every point that needs them
std::set<int>::iterator spfh_indices_itr = spfh_indices.begin ();
for (int i = 0; i < static_cast<int> (spfh_indices.size ()); ++i)
{
// Get the next point index
int p_idx = *spfh_indices_itr;
++spfh_indices_itr;
// Find the neighborhood around p_idx
if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;
// Estimate the SPFH signature around p_idx
computePointSPFHSignature (*surface_, *normals_, p_idx, i, nn_indices, hist_f1, hist_f2, hist_f3);
// Populate a lookup table for converting a point index to its corresponding row in the spfh_hist_* matrices
spfh_hist_lookup[p_idx] = i;
}
}
void addEigenMatrixRow( Eigen::MatrixXf &m )
{
Eigen::MatrixXf temp=m;
m.resize(m.rows()+1,m.cols());
m.setZero();
m.block(0,0,temp.rows(),temp.cols())=temp;
}
void calcCovarWeighed(const float *input, const double *inputWeights, int vectorLength, int nVectors, Eigen::MatrixXf &out_covMat, const Eigen::VectorXf &mean)
{
//compute covariance matrix
out_covMat.setZero();
//Eigen::MatrixXf inMat = Eigen::MatrixXf::Zero(vectorLength,nVectors);
//for (int i=0;i<vectorLength;i++){
// for(int k=0;k<nVectors;k++){
// inMat(i,k) = ((float)inputWeights[k])*(input[k*vectorLength+i]-mean[i]);
// }
//}
//out_covMat = (inMat*inMat.transpose())*(1.0/((float)nVectors-1.0));
for (int i=0; i<vectorLength; i++){
for (int j=0; j<vectorLength; j++){
double avg=0;
double iMean=mean[i];
double jMean=mean[j];
double wSum=0;
for (int k=0; k<nVectors; k++){
double w=inputWeights[k];
avg+=w*(input[k*vectorLength+i]-iMean)*(input[k*vectorLength+j]-jMean);
wSum+=w;
}
avg/=wSum;
out_covMat(i,j)=(float)avg;
}
}
}
template <typename PointInT, typename PointOutT> void
pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeIntensitySpinImage (
const PointCloudIn &cloud, float radius, float sigma,
int k,
const std::vector<int> &indices,
const std::vector<float> &squared_distances,
Eigen::MatrixXf &intensity_spin_image)
{
// Determine the number of bins to use based on the size of intensity_spin_image
int nr_distance_bins = static_cast<int> (intensity_spin_image.cols ());
int nr_intensity_bins = static_cast<int> (intensity_spin_image.rows ());
// Find the min and max intensity values in the given neighborhood
float min_intensity = std::numeric_limits<float>::max ();
float max_intensity = -std::numeric_limits<float>::max ();
for (int idx = 0; idx < k; ++idx)
{
min_intensity = (std::min) (min_intensity, cloud.points[indices[idx]].intensity);
max_intensity = (std::max) (max_intensity, cloud.points[indices[idx]].intensity);
}
float constant = 1.0f / (2.0f * sigma_ * sigma_);
// Compute the intensity spin image
intensity_spin_image.setZero ();
for (int idx = 0; idx < k; ++idx)
{
// Normalize distance and intensity values to: 0.0 <= d,i < nr_distance_bins,nr_intensity_bins
const float eps = std::numeric_limits<float>::epsilon ();
float d = static_cast<float> (nr_distance_bins) * std::sqrt (squared_distances[idx]) / (radius + eps);
float i = static_cast<float> (nr_intensity_bins) *
(cloud.points[indices[idx]].intensity - min_intensity) / (max_intensity - min_intensity + eps);
if (sigma == 0)
{
// If sigma is zero, update the histogram with no smoothing kernel
int d_idx = static_cast<int> (d);
int i_idx = static_cast<int> (i);
intensity_spin_image (i_idx, d_idx) += 1;
}
else
{
// Compute the bin indices that need to be updated (+/- 3 standard deviations)
int d_idx_min = (std::max)(static_cast<int> (floor (d - 3*sigma)), 0);
int d_idx_max = (std::min)(static_cast<int> (ceil (d + 3*sigma)), nr_distance_bins - 1);
int i_idx_min = (std::max)(static_cast<int> (floor (i - 3*sigma)), 0);
int i_idx_max = (std::min)(static_cast<int> (ceil (i + 3*sigma)), nr_intensity_bins - 1);
// Update the appropriate bins of the histogram
for (int i_idx = i_idx_min; i_idx <= i_idx_max; ++i_idx)
{
for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
{
// Compute a "soft" update weight based on the distance between the point and the bin
float w = expf (-powf (d - static_cast<float> (d_idx), 2.0f) * constant - powf (i - static_cast<float> (i_idx), 2.0f) * constant);
intensity_spin_image (i_idx, d_idx) += w;
}
}
}
}
}
void v_disparity_data_to_matrix(const FastGroundPlaneEstimator::v_disparity_data_t &image_data,
Eigen::MatrixXf &image_matrix)
{
typedef FastGroundPlaneEstimator::v_disparity_data_t::const_reference row_slice_t;
const int rows = image_data.shape()[0], cols = image_data.shape()[1];
image_matrix.setZero(rows, cols);
for(int row=0; row < rows; row +=1)
{
row_slice_t row_slice = image_data[row];
row_slice_t::const_iterator data_it = row_slice.begin();
for(int col=0; col < cols; ++data_it, col+=1)
{
//printf("%.f\n", *data_it);
image_matrix(row, col) = *data_it;
} // end "for each column"
} // end of "for each row"
return;
}
// Test getLinearAlignEquations
TEST_F(MLPTesting, getLinearAlignEquations) {
Eigen::MatrixXf A;
Eigen::MatrixXf b;
b.setZero();
c_.set_warp_identity();
c_.set_c(cv::Point2f(imgSize_/2,imgSize_/2));
mp_.extractMultilevelPatchFromImage(pyr2_,c_,nLevels_-1,true);
c_.set_c(cv::Point2f(imgSize_/2+1,imgSize_/2+1));
// NOTE: only works for patch size = 2, nLevels = 2
ASSERT_EQ(mpa_.getLinearAlignEquations(pyr2_,mp_,c_,0,nLevels_-1,A,b),true);
float meanError = (255+255*0.75)/8;
ASSERT_EQ(b(0),255-meanError);
ASSERT_EQ(b(1),-meanError);
ASSERT_EQ(b(2),-meanError);
ASSERT_EQ(b(3),-meanError);
ASSERT_EQ(b(4),255*0.75-meanError);
ASSERT_EQ(b(5),-meanError);
ASSERT_EQ(b(6),-meanError);
ASSERT_EQ(b(7),-meanError);
float mean_diff_dx = 0;
float mean_diff_dy = 0;
for(unsigned int l=0;l<nLevels_;l++){
for(unsigned int i=0;i<patchSize_;i++){
for(unsigned int j=0;j<patchSize_;j++){
mean_diff_dx += -pow(0.5,l)*mp_.patches_[l].dx_[i*patchSize_+j];
mean_diff_dy += -pow(0.5,l)*mp_.patches_[l].dy_[i*patchSize_+j];
}
}
}
mean_diff_dx /= nLevels_*patchSize_*patchSize_;
mean_diff_dy /= nLevels_*patchSize_*patchSize_;
for(unsigned int l=0;l<nLevels_;l++){
for(unsigned int i=0;i<patchSize_;i++){
for(unsigned int j=0;j<patchSize_;j++){
ASSERT_EQ(A(4*l+i*patchSize_+j,0),-pow(0.5,l)*mp_.patches_[l].dx_[i*patchSize_+j]-mean_diff_dx);
ASSERT_EQ(A(4*l+i*patchSize_+j,1),-pow(0.5,l)*mp_.patches_[l].dy_[i*patchSize_+j]-mean_diff_dy);
}
}
}
}
template <typename PointInT, typename PointOutT> void
pcl::ROPSEstimation <PointInT, PointOutT>::getDistributionMatrix (const unsigned int projection, const Eigen::Vector3f& min, const Eigen::Vector3f& max, const PointCloudIn& cloud, Eigen::MatrixXf& matrix) const
{
matrix.setZero ();
const unsigned int number_of_points = static_cast <unsigned int> (cloud.points.size ());
const unsigned int coord[3][2] = {
{0, 1},
{0, 2},
{1, 2}};
const float u_bin_length = (max (coord[projection][0]) - min (coord[projection][0])) / number_of_bins_;
const float v_bin_length = (max (coord[projection][1]) - min (coord[projection][1])) / number_of_bins_;
for (unsigned int i_point = 0; i_point < number_of_points; i_point++)
{
Eigen::Vector3f point (
cloud.points[i_point].x,
cloud.points[i_point].y,
cloud.points[i_point].z);
const float u_length = point (coord[projection][0]) - min[coord[projection][0]];
const float v_length = point (coord[projection][1]) - min[coord[projection][1]];
const float u_ratio = u_length / u_bin_length;
unsigned int row = static_cast <unsigned int> (u_ratio);
if (row == number_of_bins_) row--;
const float v_ratio = v_length / v_bin_length;
unsigned int col = static_cast <unsigned int> (v_ratio);
if (col == number_of_bins_) col--;
matrix (row, col) += 1.0f;
}
matrix /= static_cast <float> (number_of_points);
}
int main(int argc, char **argv) {
std::vector<color_keyframe::Ptr> frames;
util U;
U.load("http://localhost/corridor_map2", frames);
timestamp_t t0 = get_timestamp();
std::vector<std::pair<int, int> > overlapping_keyframes;
int size;
int workers = argc - 1;
ros::init(argc, argv, "panorama");
std::vector<
actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>*> ac_list;
for (int i = 0; i < workers; i++) {
actionlib::SimpleActionClient<rm_multi_mapper::WorkerSlamAction>* ac =
new actionlib::SimpleActionClient<
rm_multi_mapper::WorkerSlamAction>(
std::string(argv[i + 1]), true);
ac_list.push_back(ac);
}
sql::ResultSet *res;
size = frames.size();
get_pairs(overlapping_keyframes);
std::vector<rm_multi_mapper::WorkerSlamGoal> goals;
int keyframes_size = (int) overlapping_keyframes.size();
for (int k = 0; k < workers; k++) {
rm_multi_mapper::WorkerSlamGoal goal;
int last_elem = (keyframes_size / workers) * (k + 1);
if (k == workers - 1)
last_elem = keyframes_size;
for (int i = (keyframes_size / workers) * k; i < last_elem; i++) {
rm_multi_mapper::KeyframePair keyframe;
keyframe.first = overlapping_keyframes[i].first;
keyframe.second = overlapping_keyframes[i].second;
goal.Overlap.push_back(keyframe);
}
goals.push_back(goal);
}
ROS_INFO("Waiting for action server to start.");
for (int i = 0; i < workers; i++) {
ac_list[i]->waitForServer();
}
ROS_INFO("Action server started, sending goal.");
// send a goal to the action
for (int i = 0; i < workers; i++) {
ac_list[i]->sendGoal(goals[i]);
}
//wait for the action to return
std::vector<bool> finished;
for (int i = 0; i < workers; i++) {
bool finished_before_timeout = ac_list[i]->waitForResult(
ros::Duration(30.0));
finished.push_back(finished_before_timeout);
}
bool success = true;
for (int i = 0; i < workers; i++) {
success = finished[i] && success;
}
Eigen::MatrixXf acc_JtJ;
acc_JtJ.setZero(size * 6, size * 6);
Eigen::VectorXf acc_Jte;
acc_Jte.setZero(size * 6);
if (success) {
for (int i = 0; i < workers; i++) {
Eigen::MatrixXf JtJ;
JtJ.setZero(size * 6, size * 6);
Eigen::VectorXf Jte;
Jte.setZero(size * 6);
rm_multi_mapper::Vector rosJte = ac_list[i]->getResult()->Jte;
rm_multi_mapper::Matrix rosJtJ = ac_list[i]->getResult()->JtJ;
vector2eigen(rosJte, Jte);
matrix2eigen(rosJtJ, JtJ);
acc_JtJ += JtJ;
acc_Jte += Jte;
}
} else {
ROS_INFO("Action did not finish before the time out.");
std::exit(0);
}
Eigen::VectorXf update = -acc_JtJ.ldlt().solve(acc_Jte);
float iteration_max_update = std::max(std::abs(update.maxCoeff()),
std::abs(update.minCoeff()));
ROS_INFO("Max update %f", iteration_max_update);
/*for (int i = 0; i < (int)frames.size(); i++) {
frames[i]->get_pos() = Sophus::SE3f::exp(update.segment<6>(i))
* frames[i]->get_pos();
std::string query = "UPDATE `positions` SET `q0` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[0]) +
", `q1` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[1]) +
", `q2` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[2]) +
", `q3` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().so3().data()[3]) +
", `t0` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[0]) +
", `t1` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[1]) +
", `t2` = " +
boost::lexical_cast<std::string>(frames[i]->get_pos().translation()[2]) +
", `int0` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[0]) +
", `int1` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[1]) +
", `int2` = " +
boost::lexical_cast<std::string>(frames[i]->get_intrinsics().array()[2]) +
" WHERE `id` = " +
boost::lexical_cast<std::string>(i) +
";";
res = U.sql_query(query);
delete res;
}*/
timestamp_t t1 = get_timestamp();
double secs = (t1 - t0) / 1000000.0L;
std::cout << secs << std::endl;
return 0;
}
template <typename PointInT, typename GradientT, typename PointOutT> void
pcl::RIFTEstimation<PointInT, GradientT, PointOutT>::computeRIFT (
const PointCloudIn &cloud, const PointCloudGradient &gradient,
int p_idx, float radius, const std::vector<int> &indices,
const std::vector<float> &sqr_distances, Eigen::MatrixXf &rift_descriptor)
{
if (indices.empty ())
{
PCL_ERROR ("[pcl::RIFTEstimation] Null indices points passed!\n");
return;
}
// Determine the number of bins to use based on the size of rift_descriptor
int nr_distance_bins = static_cast<int> (rift_descriptor.rows ());
int nr_gradient_bins = static_cast<int> (rift_descriptor.cols ());
// Get the center point
pcl::Vector3fMapConst p0 = cloud.points[p_idx].getVector3fMap ();
// Compute the RIFT descriptor
rift_descriptor.setZero ();
for (size_t idx = 0; idx < indices.size (); ++idx)
{
// Compute the gradient magnitude and orientation (relative to the center point)
pcl::Vector3fMapConst point = cloud.points[indices[idx]].getVector3fMap ();
Eigen::Map<const Eigen::Vector3f> gradient_vector (& (gradient.points[indices[idx]].gradient[0]));
float gradient_magnitude = gradient_vector.norm ();
float gradient_angle_from_center = acosf (gradient_vector.dot ((point - p0).normalized ()) / gradient_magnitude);
if (!pcl_isfinite (gradient_angle_from_center))
gradient_angle_from_center = 0.0;
// Normalize distance and angle values to: 0.0 <= d,g < nr_distances_bins,nr_gradient_bins
const float eps = std::numeric_limits<float>::epsilon ();
float d = static_cast<float> (nr_distance_bins) * sqrtf (sqr_distances[idx]) / (radius + eps);
float g = static_cast<float> (nr_gradient_bins) * gradient_angle_from_center / (static_cast<float> (M_PI) + eps);
// Compute the bin indices that need to be updated
int d_idx_min = (std::max)(static_cast<int> (ceil (d - 1)), 0);
int d_idx_max = (std::min)(static_cast<int> (floor (d + 1)), nr_distance_bins - 1);
int g_idx_min = static_cast<int> (ceil (g - 1));
int g_idx_max = static_cast<int> (floor (g + 1));
// Update the appropriate bins of the histogram
for (int g_idx = g_idx_min; g_idx <= g_idx_max; ++g_idx)
{
// Because gradient orientation is cyclical, out-of-bounds values must wrap around
int g_idx_wrapped = ((g_idx + nr_gradient_bins) % nr_gradient_bins);
for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
{
// To avoid boundary effects, use linear interpolation when updating each bin
float w = (1.0f - fabsf (d - static_cast<float> (d_idx))) * (1.0f - fabsf (g - static_cast<float> (g_idx)));
rift_descriptor (d_idx, g_idx_wrapped) += w * gradient_magnitude;
}
}
}
// Normalize the RIFT descriptor to unit magnitude
rift_descriptor.normalize ();
}