@@ -29,23 +29,13 @@ SBA::SBA()
2929m_localWeightsSparse.resize (maxNumIts,1 .0f );
3030m_localWeightsDenseDepth.resize (maxNumIts);
3131for (unsigned int i =0 ; i < maxNumIts; i++) m_localWeightsDenseDepth[i] = (i +1 .0f );
32- m_localWeightsDenseColor.resize (maxNumIts,0 .0f );// no color
33- // for (unsigned int i = 2; i < maxNumIts; i++) m_localWeightsDenseColor[i] = 1.0f;//fr3_nstn
34- // // for tum data
35- // std::cout << "using FR2 params" << std::endl;
36- // m_localWeightsSparse.resize(maxNumIts, 1.0f);
37- // m_localWeightsDenseDepth.resize(maxNumIts, 1.0f); //for (unsigned int i = 1; i < maxNumIts; i++) m_localWeightsDenseDepth[i] = 1.0f;
38- // m_localWeightsDenseColor.resize(maxNumIts, 0.0f); //for (unsigned int i = 2; i < maxNumIts; i++) m_localWeightsDenseColor[i] = 1.0f;
39- // //for (unsigned int i = 0; i < 2; i++) m_localWeightsSparse[maxNumIts - i - 1] = 0.0f; // turn off sparse at end
32+ m_localWeightsDenseColor.resize (maxNumIts,0 .0f );
4033
4134m_globalWeightsMutex.lock ();
4235m_globalWeightsSparse.resize (maxNumIts,1 .0f );
4336m_globalWeightsDenseDepth.resize (maxNumIts,1 .0f );
44- m_globalWeightsDenseColor.resize (maxNumIts,0 .0f );// off
45- // // for tum data
46- // m_globalWeightsSparse.resize(maxNumIts, 1.0f);
47- // m_globalWeightsDenseDepth.resize(maxNumIts, 0.0f);
48- // m_globalWeightsDenseColor.resize(maxNumIts, 0.1f); //off
37+ for (unsigned int i =2 ; i < maxNumIts; i++) m_globalWeightsDenseDepth[i] = i;
38+ m_globalWeightsDenseColor.resize (maxNumIts,0 .1f );
4939
5040m_maxResidual = -1 .0f ;
5141
@@ -145,7 +135,7 @@ bool SBA::alignCUDA(SIFTImageManager* siftManager, const CUDACache* cudaCache, b
145135return removed;
146136}
147137
148- // !!! debugging
138+ // todo debugging
149139#include " GlobalAppState.h"
150140
151141#include " MatrixConversion.h"
@@ -176,78 +166,13 @@ struct std::hash<vec2ui> : public std::unary_function < vec2ui, size_t > {
176166}
177167};
178168
179- // !!!debugging
180169bool SBA::removeMaxResidualCUDA (SIFTImageManager* siftManager,unsigned int numImages,unsigned int curFrame)
181170{
182171ml::vec2ui imageIndices;
183172bool remove = m_solver->getMaxResidual (curFrame, siftManager->getGlobalCorrespondencesGPU (), imageIndices, m_maxResidual);
184173if (remove) {
185174if (GlobalBundlingState::get ().s_verbose ) std::cout <<" \t images (" << imageIndices <<" ): invalid match" << m_maxResidual << std::endl;
186175
187- /* if (false) {
188- static SensorData sd;
189- if (sd.m_frames.empty()) sd.loadFromFile(GlobalAppState::get().s_binaryDumpSensorFile);
190- //get residual corrs
191- std::vector<EntryJ> correspondences(siftManager->getNumGlobalCorrespondences());
192- MLIB_CUDA_SAFE_CALL(cudaMemcpy(correspondences.data(), siftManager->getGlobalCorrespondencesGPU(), sizeof(EntryJ)*correspondences.size(), cudaMemcpyDeviceToHost));
193- //get transforms
194- float4x4* d_transforms = NULL;
195- MLIB_CUDA_SAFE_CALL(cudaMalloc(&d_transforms, sizeof(float4x4)*numImages))
196- convertPosesToMatricesCU(d_xRot, d_xTrans, numImages, d_transforms, siftManager->getValidImagesGPU());
197- std::vector<mat4f> transforms(numImages);
198- MLIB_CUDA_SAFE_CALL(cudaMemcpy(transforms.data(), d_transforms, sizeof(float4x4)*numImages, cudaMemcpyDeviceToHost));
199- MLIB_CUDA_SAFE_FREE(d_transforms);
200- //get residuals
201- std::vector< std::pair<float, vec2ui> > residuals;
202- for (unsigned int i = 0; i < correspondences.size(); i++) {
203- const EntryJ& corr = correspondences[i];
204- if (corr.isValid()) {
205- vec3f res = transforms[corr.imgIdx_i] * vec3f(corr.pos_i.x, corr.pos_i.y, corr.pos_i.z) - transforms[corr.imgIdx_j] * vec3f(corr.pos_j.x, corr.pos_j.y, corr.pos_j.z);
206- res = math::abs(res);
207- float r = std::max(res.x, std::max(res.y, res.z));
208- residuals.push_back(std::make_pair(r, vec2ui(corr.imgIdx_i, corr.imgIdx_j)));
209- }
210- } //correspondences
211- std::sort(residuals.begin(), residuals.end(), [](const std::pair<float, vec2ui> &left, const std::pair<float, vec2ui> &right) {
212- return fabs(left.first) > fabs(right.first);
213- });
214- //find image-image
215- const float thresh = 0.05f;
216- std::unordered_map<vec2ui, float> imageImageResidualsSet;
217- for (unsigned int i = 0; i < residuals.size(); i++) {
218- if (residuals[i].first < thresh) break;
219- auto it = imageImageResidualsSet.find(residuals[i].second);
220- if (it == imageImageResidualsSet.end()) imageImageResidualsSet[residuals[i].second] = residuals[i].first;
221- else it->second = std::max(it->second, residuals[i].first);
222- }//residuals
223- const unsigned int maxToPrint = 10;
224- residuals.clear();
225- for (const auto& a : imageImageResidualsSet) residuals.push_back(std::make_pair(a.second, a.first));
226- std::sort(residuals.begin(), residuals.end(), [](const std::pair<float, vec2ui> &left, const std::pair<float, vec2ui> &right) {
227- return fabs(left.first) > fabs(right.first);
228- });
229- if (residuals.size() > maxToPrint) residuals.resize(maxToPrint);
230- std::cout << "printing " << residuals.size() << " high residual" << std::endl;
231- for (const auto& impair : residuals) {
232- vec3uc* im1 = sd.decompressColorAlloc(impair.second.x * 10);
233- vec3uc* im2 = sd.decompressColorAlloc(impair.second.y * 10);
234- ColorImageR8G8B8 image1(sd.m_colorWidth, sd.m_colorHeight, im1);
235- ColorImageR8G8B8 image2(sd.m_colorWidth, sd.m_colorHeight, im2);
236- std::free(im1);std::free(im2);
237- SiftVisualization::printMatch("debug/maxres/" + std::to_string((int)(100 * impair.first)) + "_" + std::to_string(impair.second.x) + "-" + std::to_string(impair.second.y) + ".png",
238- impair.second, correspondences, image1, image2, sd.m_calibrationColor.m_intrinsic);
239- unsigned short* d1 = sd.decompressDepthAlloc(impair.second.x * 10);
240- unsigned short* d2 = sd.decompressDepthAlloc(impair.second.y * 10);
241- DepthImage32 depth1(DepthImage16(sd.m_depthWidth, sd.m_depthHeight, d1));
242- DepthImage32 depth2(DepthImage16(sd.m_depthWidth, sd.m_depthHeight, d2));
243- std::free(d1);std::free(d2);
244- image1.resize(depth1.getWidth(), depth1.getHeight());image2.resize(depth2.getWidth(), depth2.getHeight());
245- SiftVisualization::saveKeyMatchToPointCloud("debug/maxres/" + std::to_string((int)(100 * impair.first)) + "_" + std::to_string(impair.second.x) + "-" + std::to_string(impair.second.y),
246- impair.second, correspondences, depth1, image1, depth2, image2, transforms, sd.m_calibrationDepth.m_intrinsic.getInverse());
247- } //print
248- std::cout << "waiting..." << std::endl;
249- getchar();
250- }*/
251176#ifdef NEW_GUIDED_REMOVE
252177const std::vector<vec2ui>& imPairsToRemove = m_solver->getGuidedMaxResImagesToRemove ();
253178if (imPairsToRemove.empty ()) {
@@ -263,29 +188,6 @@ bool SBA::removeMaxResidualCUDA(SIFTImageManager* siftManager, unsigned int numI
263188}
264189}
265190#else
266- // //!!!debugging
267- // {
268- // //std::ofstream s("debug/logs/" + std::to_string(siftManager->getNumImages()) + "-" + std::to_string(siftManager->getCurrentFrame()) + ".txt");
269- // //s << imageIndices << " : " << m_maxResidual << std::endl;
270- // //if (siftManager->getCurrentFrame() >= 70) {
271- // //float4x4* d_transforms = NULL;
272- // //MLIB_CUDA_SAFE_CALL(cudaMalloc(&d_transforms, sizeof(float4x4)*numImages));
273- // //convertPosesToMatricesCU(d_xRot, d_xTrans, numImages, d_transforms, siftManager->getValidImagesGPU());
274- // //std::vector<mat4f> transforms(numImages);
275- // //MLIB_CUDA_SAFE_CALL(cudaMemcpy(transforms.data(), d_transforms, sizeof(float4x4)*numImages, cudaMemcpyDeviceToHost));
276- // //const std::vector<int>& valid = siftManager->getValidImages();
277- // //for (unsigned int i = 0; i < numImages; i++) if (valid[i] == 0) transforms[i].setZero(-std::numeric_limits<float>::infinity());
278- // //BinaryDataStreamFile ofs("debug/logs/" + std::to_string(siftManager->getCurrentFrame()) + ".trajectory", true);
279- // //ofs << transforms; ofs.closeStream();
280- // //MLIB_CUDA_SAFE_FREE(d_transforms);
281- // //siftManager->saveToFile("debug/logs/" + std::to_string(siftManager->getCurrentFrame()) + ".sift");
282- // //}
283- // //if (siftManager->getCurrentFrame() == 71) {
284- // //std::cout << "waiting..." << std::endl;
285- // //getchar();
286- // //}
287- // }
288- // //!!!debugging
289191
290192// invalidate correspondence
291193siftManager->InvalidateImageToImageCU (make_uint2 (imageIndices.x , imageIndices.y ));
@@ -298,24 +200,11 @@ bool SBA::removeMaxResidualCUDA(SIFTImageManager* siftManager, unsigned int numI
298200
299201return true ;
300202}
301- // else std::cout << "\thighest residual " << m_maxResidual << " from images (" << imageIndices << ")" << std::endl;
302203return false ;
303204}
304205
305206void SBA::printConvergence (const std::string& filename)const
306207{
307- // if (m_recordedConvergence.empty()) return;
308- // std::ofstream s(filename);
309- // s << m_recordedConvergence.size() << " optimizations" << std::endl;
310- // s << std::endl;
311- // for (unsigned int i = 0; i < m_recordedConvergence.size(); i++) {
312- // s << "[ opt# " << i << " ]" << std::endl;
313- // for (unsigned int k = 0; k < m_recordedConvergence[i].size(); k++)
314- // s << "\titer " << k << ": " << m_recordedConvergence[i][k] << std::endl;
315- // s << std::endl;
316- // }
317- // s.close();
318-
319208if (m_recordedConvergence.empty ())return ;
320209std::ofstreams (filename);
321210for (unsigned int i =0 ; i < m_recordedConvergence.size (); i++) {