typename MaskedHistogramGenerator<TBinValue, TQuadrantProperties>::HistogramType MaskedHistogramGenerator<TBinValue, TQuadrantProperties>::ComputeMaskedScalarImageHistogram
(const TImage* const image, const itk::ImageRegion<2>& imageRegion,
const Mask* const mask, const itk::ImageRegion<2>& maskRegion, const unsigned int numberOfBins,
const TRangeValue& rangeMin, const TRangeValue& rangeMax, const bool allowOutside, const HoleMaskPixelTypeEnum& maskValue)
{
std::vector<itk::Index<2> > maskIndices = ITKHelpers::GetPixelsWithValueInRegion(mask, maskRegion, maskValue);
// Compute the corresponding locations in the imageRegion
std::vector<itk::Index<2> > imageIndices(maskIndices.size());
itk::Offset<2> maskRegionToImageRegionOffset = imageRegion.GetIndex() - maskRegion.GetIndex(); // The offset from the maskRegion to the imageRegion
for(size_t i = 0; i < maskIndices.size(); ++i)
{
imageIndices[i] = maskIndices[i] + maskRegionToImageRegionOffset;
}
HistogramType histogram;
// Get this channels masked scalar values
std::vector<typename TImage::PixelType> validPixels =
ITKHelpers::GetPixelValues(image, imageIndices);
// Compute the histogram of the scalar values
if(allowOutside)
{
histogram = HistogramGeneratorType::ScalarHistogramAllowOutside(validPixels, numberOfBins, rangeMin, rangeMax);
}
else
{
histogram = HistogramGeneratorType::ScalarHistogram(validPixels, numberOfBins, rangeMin, rangeMax);
}
return histogram;
}
typename MaskedHistogramGenerator<TBinValue, TQuadrantProperties>::HistogramType MaskedHistogramGenerator<TBinValue, TQuadrantProperties>::ComputeMaskedImage1DHistogram(
const itk::VectorImage<TComponent, 2>* image,
const itk::ImageRegion<2>& imageRegion,
const Mask* const mask,
const itk::ImageRegion<2>& maskRegion,
const unsigned int numberOfBinsPerDimension,
const TRangeContainer& rangeMins,
const TRangeContainer& rangeMaxs, const bool allowOutside, const HoleMaskPixelTypeEnum& maskValue)
{
// For VectorImage, we must use VectorImageToImageAdaptor
typedef itk::VectorImage<TComponent, 2> ImageType;
typedef itk::Image<TComponent, 2> ScalarImageType;
std::vector<itk::Index<2> > maskIndices = ITKHelpers::GetPixelsWithValueInRegion(mask, maskRegion, maskValue);
// Compute the corresponding locations in the imageRegion
std::vector<itk::Index<2> > imageIndices(maskIndices.size());
itk::Offset<2> maskRegionToImageRegionOffset = imageRegion.GetIndex() - maskRegion.GetIndex(); // The offset from the maskRegion to the imageRegion
for(size_t i = 0; i < maskIndices.size(); ++i)
{
imageIndices[i] = maskIndices[i] + maskRegionToImageRegionOffset;
}
HistogramType concatenatedHistograms;
for(unsigned int channel = 0; channel < image->GetNumberOfComponentsPerPixel(); ++channel)
{
// Extract the channel
typedef itk::VectorImageToImageAdaptor<TComponent, 2> ImageAdaptorType;
typename ImageAdaptorType::Pointer adaptor = ImageAdaptorType::New();
adaptor->SetExtractComponentIndex(channel);
adaptor->SetImage(const_cast<ImageType*>(image));
// Get this channels masked scalar values
std::vector<typename ScalarImageType::PixelType> validPixels =
ITKHelpers::GetPixelValues(adaptor.GetPointer(), imageIndices);
// Compute the histogram of the scalar values
HistogramType histogram;
if(allowOutside)
{
histogram = HistogramGeneratorType::ScalarHistogramAllowOutside(validPixels, numberOfBinsPerDimension,
rangeMins[channel], rangeMaxs[channel]);
}
else
{
histogram = HistogramGeneratorType::ScalarHistogram(validPixels, numberOfBinsPerDimension,
rangeMins[channel], rangeMaxs[channel]);
}
concatenatedHistograms.insert(concatenatedHistograms.end(), histogram.begin(), histogram.end());
}
return concatenatedHistograms;
}
void CUDASolverBundling::computeMaxResidual(SolverInput& solverInput, SolverParameters& parameters, unsigned int revalidateIdx)
{
if (m_timer) m_timer->startEvent(__FUNCTION__);
if (parameters.weightSparse > 0.0f) {
evalMaxResidual(solverInput, m_solverState, m_solverExtra, parameters, NULL);//m_timer);
// copy to cpu
unsigned int n = (solverInput.numberOfCorrespondences + THREADS_PER_BLOCK - 1) / THREADS_PER_BLOCK;
cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidual, m_solverExtra.d_maxResidual, sizeof(float) * n, cudaMemcpyDeviceToHost));
cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidualIndex, m_solverExtra.d_maxResidualIndex, sizeof(int) * n, cudaMemcpyDeviceToHost));
// compute max
float maxResidual = 0.0f; int maxResidualIndex = 0;
for (unsigned int i = 0; i < n; i++) {
if (maxResidual < m_solverExtra.h_maxResidual[i]) {
maxResidual = m_solverExtra.h_maxResidual[i];
maxResidualIndex = m_solverExtra.h_maxResidualIndex[i];
}
}
#ifdef NEW_GUIDED_REMOVE
//if (solverInput.numberOfImages == 51) {
// SensorData sd; sd.loadFromFile("../data/iclnuim/aliv2.sens");
// std::vector<mat4f> trajectory(solverInput.numberOfImages);
// MLIB_CUDA_SAFE_CALL(cudaMemcpy(trajectory.data(), d_transforms, sizeof(mat4f)*trajectory.size(), cudaMemcpyDeviceToHost));
// sd.saveToPointCloud("debug/tmp.ply", trajectory, 0, solverInput.numberOfImages*10, 10, true);
// int a = 5;
//}
m_maxResImPairs.clear();
if (maxResidual > GUIDED_SEARCH_MAX_RES_THRESH) {
parameters.highResidualThresh = std::min(std::max(0.2f * maxResidual, 0.1f), 0.4f);
collectHighResiduals(solverInput, m_solverState, m_solverExtra, parameters, m_timer);
unsigned int highResCount;
cutilSafeCall(cudaMemcpy(&highResCount, m_solverState.d_countHighResidual, sizeof(unsigned int), cudaMemcpyDeviceToHost));
n = std::min(highResCount, (m_maxCorrPerImage*m_maxNumberOfImages + THREADS_PER_BLOCK - 1) / THREADS_PER_BLOCK);
cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidual, m_solverExtra.d_maxResidual, sizeof(float) * n, cudaMemcpyDeviceToHost));
cutilSafeCall(cudaMemcpy(m_solverExtra.h_maxResidualIndex, m_solverExtra.d_maxResidualIndex, sizeof(int) * n, cudaMemcpyDeviceToHost));
if (n > 1) {
// check high residuals with previous trajectory as reference //TODO MAKE EFFICIENT
std::vector<float4x4> transforms(solverInput.numberOfImages);
MLIB_CUDA_SAFE_CALL(cudaMemcpy(transforms.data(), d_transforms, sizeof(float4x4)*solverInput.numberOfImages, cudaMemcpyDeviceToHost));
std::unordered_map<vec2ui, float> residualMap; //TODO should be something better than this...
std::unordered_map<vec2ui, float> allCollectedResidualMap; //debugging
std::vector<EntryJ> corrs(n);
for (unsigned int i = 0; i < n; i++) {
MLIB_CUDA_SAFE_CALL(cudaMemcpy(corrs.data() + i, solverInput.d_correspondences + m_solverExtra.h_maxResidualIndex[i], sizeof(EntryJ), cudaMemcpyDeviceToHost));
const EntryJ& h_corr = corrs[i];
vec2ui imageIndices(h_corr.imgIdx_i, h_corr.imgIdx_j);
//compute res at previous
if (h_corr.imgIdx_j == solverInput.numberOfImages - 1 && std::abs((int)h_corr.imgIdx_i - (int)h_corr.imgIdx_j) > 10) { //introduced by latest image
float3 prevRes = fabs(transforms[h_corr.imgIdx_i] * h_corr.pos_i - transforms[h_corr.imgIdx_j] * h_corr.pos_j); //eval new corrs with previous trajectory
float prevMaxRes = fmaxf(prevRes.z, fmaxf(prevRes.x, prevRes.y));
if (prevMaxRes > 1.5f*m_solverExtra.h_maxResidual[i]) {
auto it = residualMap.find(imageIndices);
if (it == residualMap.end()) residualMap[imageIndices] = m_solverExtra.h_maxResidual[i];
else it->second = std::max(m_solverExtra.h_maxResidual[i], it->second);
}
}
else if (h_corr.imgIdx_j == revalidateIdx && std::abs((int)h_corr.imgIdx_i - (int)h_corr.imgIdx_j) > 10) { //introduced by latest revalidate
auto it = residualMap.find(imageIndices);
if (it == residualMap.end()) residualMap[imageIndices] = m_solverExtra.h_maxResidual[i];
else it->second = std::max(m_solverExtra.h_maxResidual[i], it->second);
}
auto it = allCollectedResidualMap.find(imageIndices);
if (it == allCollectedResidualMap.end()) allCollectedResidualMap[imageIndices] = m_solverExtra.h_maxResidual[i];
else it->second = std::max(m_solverExtra.h_maxResidual[i], it->second);
}
if (!residualMap.empty()) { //debug print
unsigned int rep = residualMap.begin()->first.x;
std::cout << "rep: (" << rep << ", " << solverInput.numberOfImages - 1 << ")" << std::endl;
for (const auto& r : residualMap) m_maxResImPairs.push_back(r.first);
////one extra solve
//parameters.nNonLinearIterations = 1;
//solveBundlingStub(solverInput, m_solverState, parameters, m_solverExtra, NULL, m_timer);
////!!!debugging
//{
// static SensorData sd;
// if (sd.m_frames.empty()) sd.loadFromFile("../data/iclnuim/aliv2.sens");
// std::vector<mat4f> trajectory(solverInput.numberOfImages);
// MLIB_CUDA_SAFE_CALL(cudaMemcpy(trajectory.data(), d_transforms, sizeof(mat4f)*trajectory.size(), cudaMemcpyDeviceToHost));
// sd.saveToPointCloud("debug/tmp/" + std::to_string(solverInput.numberOfImages) + "-init.ply", trajectory, 0, solverInput.numberOfImages*10, 10, true);
// convertLiePosesToMatricesCU(m_solverState.d_xRot, m_solverState.d_xTrans, solverInput.numberOfImages, d_transforms, m_solverState.d_xTransformInverses);
// MLIB_CUDA_SAFE_CALL(cudaMemcpy(trajectory.data(), d_transforms, sizeof(mat4f)*trajectory.size(), cudaMemcpyDeviceToHost));
// sd.saveToPointCloud("debug/tmp/" + std::to_string(solverInput.numberOfImages) + "-opt.ply", trajectory, 0, solverInput.numberOfImages*10, 10, true);
// int a = 5;
//}
////!!!debugging
}
//!!!debugging
//std::vector<std::pair<vec2ui, float>> residuals(allCollectedResidualsMap.begin(), allCollectedResidualsMap.end());
//std::sort(residuals.begin(), residuals.end(), [](const std::pair<vec2ui, float> &left, const std::pair<vec2ui, float> &right) { //debugging only
// return left.second > right.second;
//});
//if (m_maxResImPairs.size() > 1) {
// std::ofstream s("debug/_logs/" + std::to_string(solverInput.numberOfImages) + "_" + std::to_string(m_maxResImPairs.front().x) + "-" + std::to_string(m_maxResImPairs.front().y) + ".txt");
// s << "# im pairs to remove = " << m_maxResImPairs.size() << ", res thresh = " << parameters.highResidualThresh << std::endl;
// for (unsigned int i = 0; i < m_maxResImPairs.size(); i++) s << m_maxResImPairs[i] << std::endl;
// s.close();
//}
//!!!debugging
}
}
#endif
m_solverExtra.h_maxResidual[0] = maxResidual;
m_solverExtra.h_maxResidualIndex[0] = maxResidualIndex;
}
else {
m_solverExtra.h_maxResidual[0] = 0.0f;
m_solverExtra.h_maxResidualIndex[0] = 0;
}
if (m_timer) m_timer->endEvent();
}
