Skip to content

Commit

Permalink
solved covariance merging problem
Browse files Browse the repository at this point in the history
  • Loading branch information
@amock
amock committed Oct 28, 2022
1 parent 32ecd57 commit 6d6fe32
Show file tree
Hide file tree
Showing 18 changed files with 483 additions and 615 deletions.
60 changes: 60 additions & 0 deletions include/rmcl/correction/CorrectionResults.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -40,6 +40,66 @@

namespace rmcl {

template<typename MemT>
struct CorrespondencesView
{
rmagine::MemoryView<rmagine::Vector, MemT> dataset_points;
rmagine::MemoryView<rmagine::Vector, MemT> model_points;
rmagine::MemoryView<unsigned int, MemT> corr_valid;
};


template<typename MemT>
struct Correspondences
{
rmagine::Memory<rmagine::Vector, MemT> dataset_points;
rmagine::Memory<rmagine::Vector, MemT> model_points;
rmagine::Memory<unsigned int, MemT> corr_valid;




inline CorrespondencesView<MemT> slice(
unsigned int start, unsigned int end) const
{
return {
dataset_points.slice(start, end),
model_points.slice(start, end),
corr_valid.slice(start, end)
};
}

inline CorrespondencesView<MemT> view() const
{
return {
dataset_points,
model_points,
corr_valid
};
}

// Enable this function?
// inline std::vector<CorrespondencesView<MemT> > split(
// unsigned int N) const
// {
// std::vector<CorrespondencesView<MemT> > ret;

// unsigned int batch_size = dataset_points.size() / N;

// for(unsigned int i=0; i<N; i++)
// {
// ret.push_back({
// dataset_points(i * batch_size, (i+1) * batch_size),
// model_points(i * batch_size, (i+1) * batch_size),
// corr_valid(i * batch_size, (i+1) * batch_size)
// });
// }

// return ret;
// }
};


template<typename MemT>
struct CorrectionPreResults
{
Expand Down
27 changes: 0 additions & 27 deletions include/rmcl/correction/PinholeCorrectorEmbree.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -111,15 +111,6 @@ class PinholeCorrectorEmbree
rmagine::MemoryView<unsigned int, rmagine::RAM>& Ncorr
);

// Next: reuse the already computed correspondences
// void computeCovs(
// const rmagine::MemoryView<rmagine::Point> dataset_points,
// const rmagine::MemoryView<rmagine::Point> model_points,
// const rmagine::MemoryView<unsigned int> corr_valid,
// rmagine::MemoryView<rmagine::Matrix3x3, rmagine::RAM>& Cs,
// rmagine::MemoryView<unsigned int, rmagine::RAM>& Ncorr
// );

void computeCovs(
const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms,
CorrectionPreResults<rmagine::RAM>& res
Expand Down Expand Up @@ -172,24 +163,6 @@ class PinholeCorrectorEmbree

// TODO: currently unused
rmagine::SVDPtr m_svd;

private:
// different implementations for testing
void computeCovsSequential(
const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms,
rmagine::MemoryView<rmagine::Vector, rmagine::RAM>& data_means,
rmagine::MemoryView<rmagine::Vector, rmagine::RAM>& model_means,
rmagine::MemoryView<rmagine::Matrix3x3, rmagine::RAM>& Cs,
rmagine::MemoryView<unsigned int, rmagine::RAM>& Ncorr
);

void computeCovsOuterInnerParallel(
const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms,
rmagine::MemoryView<rmagine::Vector, rmagine::RAM>& data_means,
rmagine::MemoryView<rmagine::Vector, rmagine::RAM>& model_means,
rmagine::MemoryView<rmagine::Matrix3x3, rmagine::RAM>& Cs,
rmagine::MemoryView<unsigned int, rmagine::RAM>& Ncorr
);
};

using PinholeCorrectorEmbreePtr = std::shared_ptr<PinholeCorrectorEmbree>;
Expand Down
10 changes: 10 additions & 0 deletions include/rmcl/correction/SphereCorrectorEmbree.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,16 @@ class SphereCorrectorEmbree
rmagine::Memory<unsigned int>& corr_valid
);

// TODO: add them to other classes
void findSPC(
const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms,
Correspondences<rmagine::RAM>& corr
);

Correspondences<rmagine::RAM> findSPC(
const rmagine::MemoryView<rmagine::Transform, rmagine::RAM>& Tbms);


// TODO: add properly - rmagine
inline CorrectionParams params() const
{
Expand Down
15 changes: 3 additions & 12 deletions src/nodes/tests/cov_online.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,9 @@ void fill_ones(rm::MemoryView<unsigned int> mask)

int main(int argc, char** argv)
{
rm::Mem<rm::Vector> dataset(128);
// fill_random(dataset);
fill_sequence(dataset);
rm::Mem<rm::Vector> dataset(10000);
fill_random(dataset);
// fill_sequence(dataset);

rm::Mem<rm::Vector> model(dataset.size());

Expand All @@ -60,7 +60,6 @@ int main(int argc, char** argv)
model[i] = T * dataset[i];
}


rm::StopWatchHR sw;
double el;
rm::Mem<rm::Vector> ds(1);
Expand Down Expand Up @@ -113,7 +112,6 @@ int main(int argc, char** argv)
rm::Mem<rm::Matrix3x3, rm::VRAM_CUDA> Cs_(1);
rm::Mem<unsigned int, rm::VRAM_CUDA> Ncorr_(1);


std::cout << "means_covs_batched" << std::endl;
sw();
rmcl::means_covs_batched(dataset_, model_, mask_, ds_, ms_, Cs_, Ncorr_);
Expand Down Expand Up @@ -148,12 +146,5 @@ int main(int argc, char** argv)
std::cout << Ncorr[0] << std::endl;
}








return 0;
}
99 changes: 55 additions & 44 deletions src/rmcl/correction/O1DnCorrectorEmbree.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,18 +128,28 @@ CorrectionResults<rm::RAM> O1DnCorrectorEmbree::correct(
const Vector pmesh_b = Tsb * pmesh_s;

// Online update: Covariance and means
// - https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
// - wrong: https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
// use the following equations instead
{
Ncorr++;
const float N = static_cast<float>(Ncorr);
const float N_1 = static_cast<float>(Ncorr);
const float N = static_cast<float>(Ncorr + 1);
const float w1 = N_1/N;
const float w2 = 1.0/N;

const Vector dD = preal_b - Dmean;
const Vector dM = pmesh_b - Mmean;
const rm::Vector d_mean_old = Dmean;
const rm::Vector m_mean_old = Mmean;

// reduction
Dmean += dD / N;
Mmean += dM / N;
C += dM.multT(dD);
const rm::Vector d_mean_new = d_mean_old * w1 + preal_b * w2;
const rm::Vector m_mean_new = m_mean_old * w1 + pmesh_b * w2;

auto P1 = (pmesh_b - m_mean_new).multT(preal_b - d_mean_new);
auto P2 = (m_mean_old - m_mean_new).multT(d_mean_old - d_mean_new);

// write
Dmean = d_mean_new;
Mmean = m_mean_new;
C = C * w1 + P1 * w2 + P2 * w1;
Ncorr = Ncorr + 1;
}
}
}
Expand All @@ -150,9 +160,6 @@ CorrectionResults<rm::RAM> O1DnCorrectorEmbree::correct(

if(Ncorr > 0)
{
const float Ncorr_f = static_cast<float>(Ncorr);
C /= Ncorr_f;

Matrix3x3 U, V, S;

{ // the only Eigen code left
Expand Down Expand Up @@ -285,39 +292,38 @@ void O1DnCorrectorEmbree::computeCovs(
const Vector pmesh_b = Tsb * pmesh_s;

// Online update: Covariance and means
// - https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance

// - wrong: https://en.wikipedia.org/wiki/Algorithms_for_calculating_variance
// use the following equations instead
{
const Vector dD = preal_b - Dmean;
const Vector dM = pmesh_b - Mmean;
float N = static_cast<float>(Ncorr_ + 1);

// reduction
Ncorr_++;
Mmean += dM / N;
Dmean += dD / N;
C += dM.multT(dD);
const float N_1 = static_cast<float>(Ncorr_);
const float N = static_cast<float>(Ncorr_ + 1);
const float w1 = N_1/N;
const float w2 = 1.0/N;

const rm::Vector d_mean_old = Dmean;
const rm::Vector m_mean_old = Mmean;

const rm::Vector d_mean_new = d_mean_old * w1 + preal_b * w2;
const rm::Vector m_mean_new = m_mean_old * w1 + pmesh_b * w2;

auto P1 = (pmesh_b - m_mean_new).multT(preal_b - d_mean_new);
auto P2 = (m_mean_old - m_mean_new).multT(d_mean_old - d_mean_new);

// write
Dmean = d_mean_new;
Mmean = m_mean_new;
C = C * w1 + P1 * w2 + P2 * w1;
Ncorr_ = Ncorr_ + 1;
}
}
}
}
}

Ncorr[pid] = Ncorr_;

if(Ncorr_ > 0)
{
const float Ncorr_f = static_cast<float>(Ncorr_);
C /= Ncorr_f;

ds[pid] = Dmean;
ms[pid] = Mmean;
Cs[pid] = C;
} else {
ds[pid] = {0.0, 0.0, 0.0};
ms[pid] = {0.0, 0.0, 0.0};
Cs[pid].setZeros();
}
ds[pid] = Dmean;
ms[pid] = Mmean;
Cs[pid] = C;
}
}

Expand Down Expand Up @@ -376,6 +382,8 @@ void O1DnCorrectorEmbree::findSPC(
if(range_real < m_model->range.min
|| range_real > m_model->range.max)
{
dataset_points[glob_id] = {0.0f, 0.0f, 0.0f};
model_points[glob_id] = {0.0f, 0.0f, 0.0f};
corr_valid[glob_id] = 0;
continue;
}
Expand Down Expand Up @@ -434,19 +442,22 @@ void O1DnCorrectorEmbree::findSPC(

const float distance = (pmesh_s - preal_s).l2norm();

if(distance < max_distance)
{
// convert back to base (sensor shared coordinate system)
const Vector preal_b = Tsb * preal_s;
const Vector pmesh_b = Tsb * pmesh_s;
// convert back to base (sensor shared coordinate system)
const Vector preal_b = Tsb * preal_s;
const Vector pmesh_b = Tsb * pmesh_s;

dataset_points[glob_id] = preal_b;
model_points[glob_id] = pmesh_b;
dataset_points[glob_id] = preal_b;
model_points[glob_id] = pmesh_b;

if(distance < max_distance)
{
corr_valid[glob_id] = 1;
} else {
corr_valid[glob_id] = 0;
}
} else {
dataset_points[glob_id] = {0.0f, 0.0f, 0.0f};
model_points[glob_id] = {0.0f, 0.0f, 0.0f};
corr_valid[glob_id] = 0;
}
}
Expand Down
Loading

0 comments on commit 6d6fe32

Please sign in to comment.