From 0ce6ef8d8d9261d753b57abea11ba0e5b3dad6e3 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Fri, 14 Oct 2016 15:24:24 -0400 Subject: [PATCH] CLAMS: cleanup eigen_extensions.h, models can be saved/loaded in binary (*.bin) or ascii (*.txt) formats --- .../clams/discrete_depth_distortion_model.h | 8 +- .../clams/discrete_depth_distortion_model.cpp | 134 ++++++++---- .../clams/eigen_extensions/eigen_extensions.h | 202 +++--------------- guilib/src/DepthCalibrationDialog.cpp | 4 +- guilib/src/ExportCloudsDialog.cpp | 2 +- guilib/src/PreferencesDialog.cpp | 2 +- 6 files changed, 129 insertions(+), 223 deletions(-) diff --git a/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h b/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h index a501607b..bfc974f1 100644 --- a/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h +++ b/corelib/include/rtabmap/core/clams/discrete_depth_distortion_model.h @@ -49,8 +49,8 @@ namespace clams int index(double z) const; void undistort(double* z) const; void interpolatedUndistort(double* z) const; - void serialize(std::ostream& out) const; - void deserialize(std::istream& in); + void serialize(std::ostream& out, bool ascii) const; + void deserialize(std::istream& in, bool ascii); protected: double max_dist_; @@ -88,8 +88,8 @@ namespace clams void addExample(int v, int u, double ground_truth, double measurement); void save(const std::string& path) const; void load(const std::string& path); - void serialize(std::ostream& out) const; - void deserialize(std::istream& in); + void serialize(std::ostream& out, bool ascii) const; + void deserialize(std::istream& in, bool ascii); cv::Mat visualize(const std::string& path = "") const; int getWidth() const {return width_;} diff --git a/corelib/src/clams/discrete_depth_distortion_model.cpp b/corelib/src/clams/discrete_depth_distortion_model.cpp index b8c72fe2..d57a486d 100644 --- a/corelib/src/clams/discrete_depth_distortion_model.cpp +++ b/corelib/src/clams/discrete_depth_distortion_model.cpp @@ -31,6 +31,7 @@ RTAB-Map integration: Mathieu Labbe #include "rtabmap/core/clams/frame_projector.h" #include #include +#include #include "eigen_extensions/eigen_extensions.h" using namespace std; @@ -97,26 +98,52 @@ namespace clams *z *= mult; } - void DiscreteFrustum::serialize(std::ostream& out) const + void DiscreteFrustum::serialize(std::ostream& out, bool ascii) const { - eigen_extensions::serializeScalar(max_dist_, out); - eigen_extensions::serializeScalar(num_bins_, out); - eigen_extensions::serializeScalar(bin_depth_, out); - eigen_extensions::serialize(counts_, out); - eigen_extensions::serialize(total_numerators_, out); - eigen_extensions::serialize(total_denominators_, out); - eigen_extensions::serialize(multipliers_, out); + if(ascii) + { + eigen_extensions::serializeScalarASCII(max_dist_, out); + eigen_extensions::serializeScalarASCII(num_bins_, out); + eigen_extensions::serializeScalarASCII(bin_depth_, out); + eigen_extensions::serializeASCII(counts_, out); + eigen_extensions::serializeASCII(total_numerators_, out); + eigen_extensions::serializeASCII(total_denominators_, out); + eigen_extensions::serializeASCII(multipliers_, out); + } + else + { + eigen_extensions::serializeScalar(max_dist_, out); + eigen_extensions::serializeScalar(num_bins_, out); + eigen_extensions::serializeScalar(bin_depth_, out); + eigen_extensions::serialize(counts_, out); + eigen_extensions::serialize(total_numerators_, out); + eigen_extensions::serialize(total_denominators_, out); + eigen_extensions::serialize(multipliers_, out); + } } - void DiscreteFrustum::deserialize(std::istream& in) + void DiscreteFrustum::deserialize(std::istream& in, bool ascii) { - eigen_extensions::deserializeScalar(in, &max_dist_); - eigen_extensions::deserializeScalar(in, &num_bins_); - eigen_extensions::deserializeScalar(in, &bin_depth_); - eigen_extensions::deserialize(in, &counts_); - eigen_extensions::deserialize(in, &total_numerators_); - eigen_extensions::deserialize(in, &total_denominators_); - eigen_extensions::deserialize(in, &multipliers_); + if(ascii) + { + eigen_extensions::deserializeScalarASCII(in, &max_dist_); + eigen_extensions::deserializeScalarASCII(in, &num_bins_); + eigen_extensions::deserializeScalarASCII(in, &bin_depth_); + eigen_extensions::deserializeASCII(in, &counts_); + eigen_extensions::deserializeASCII(in, &total_numerators_); + eigen_extensions::deserializeASCII(in, &total_denominators_); + eigen_extensions::deserializeASCII(in, &multipliers_); + } + else + { + eigen_extensions::deserializeScalar(in, &max_dist_); + eigen_extensions::deserializeScalar(in, &num_bins_); + eigen_extensions::deserializeScalar(in, &bin_depth_); + eigen_extensions::deserialize(in, &counts_); + eigen_extensions::deserialize(in, &total_numerators_); + eigen_extensions::deserialize(in, &total_denominators_); + eigen_extensions::deserialize(in, &multipliers_); + } UDEBUG("Frustum: max_dist=%f", max_dist_); UDEBUG("Frustum: num_bins=%d", num_bins_); UDEBUG("Frustum: bin_depth=%f", bin_depth_); @@ -267,60 +294,91 @@ namespace clams void DiscreteDepthDistortionModel::load(const std::string& path) { + bool ascii = UFile::getExtension(path).compare("txt") == 0; ifstream f; f.open(path.c_str()); if(!f.is_open()) { cerr << "Failed to open " << path << endl; assert(f.is_open()); } - deserialize(f); + deserialize(f, ascii); f.close(); } void DiscreteDepthDistortionModel::save(const std::string& path) const { + bool ascii = UFile::getExtension(path).compare("txt") == 0; ofstream f; f.open(path.c_str()); if(!f.is_open()) { cerr << "Failed to open " << path << endl; assert(f.is_open()); } - serialize(f); + serialize(f, ascii); f.close(); } - void DiscreteDepthDistortionModel::serialize(std::ostream& out) const + void DiscreteDepthDistortionModel::serialize(std::ostream& out, bool ascii) const { out << "DiscreteDepthDistortionModel v01" << endl; - eigen_extensions::serializeScalar(width_, out); - eigen_extensions::serializeScalar(height_, out); - eigen_extensions::serializeScalar(bin_width_, out); - eigen_extensions::serializeScalar(bin_height_, out); - eigen_extensions::serializeScalar(bin_depth_, out); - eigen_extensions::serializeScalar(num_bins_x_, out); - eigen_extensions::serializeScalar(num_bins_y_, out); - eigen_extensions::serializeScalar(training_samples_, out); + if(ascii) + { + eigen_extensions::serializeScalarASCII(width_, out); + eigen_extensions::serializeScalarASCII(height_, out); + eigen_extensions::serializeScalarASCII(bin_width_, out); + eigen_extensions::serializeScalarASCII(bin_height_, out); + eigen_extensions::serializeScalarASCII(bin_depth_, out); + eigen_extensions::serializeScalarASCII(num_bins_x_, out); + eigen_extensions::serializeScalarASCII(num_bins_y_, out); + eigen_extensions::serializeScalarASCII(training_samples_, out); + } + else + { + eigen_extensions::serializeScalar(width_, out); + eigen_extensions::serializeScalar(height_, out); + eigen_extensions::serializeScalar(bin_width_, out); + eigen_extensions::serializeScalar(bin_height_, out); + eigen_extensions::serializeScalar(bin_depth_, out); + eigen_extensions::serializeScalar(num_bins_x_, out); + eigen_extensions::serializeScalar(num_bins_y_, out); + eigen_extensions::serializeScalar(training_samples_, out); + } + for(int y = 0; y < num_bins_y_; ++y) for(int x = 0; x < num_bins_x_; ++x) - frustums_[y][x]->serialize(out); + frustums_[y][x]->serialize(out, ascii); } - void DiscreteDepthDistortionModel::deserialize(std::istream& in) + void DiscreteDepthDistortionModel::deserialize(std::istream& in, bool ascii) { UDEBUG(""); string buf; getline(in, buf); UDEBUG("buf=%s", buf.c_str()); assert(buf == "DiscreteDepthDistortionModel v01"); - eigen_extensions::deserializeScalar(in, &width_); - eigen_extensions::deserializeScalar(in, &height_); - eigen_extensions::deserializeScalar(in, &bin_width_); - eigen_extensions::deserializeScalar(in, &bin_height_); - eigen_extensions::deserializeScalar(in, &bin_depth_); - eigen_extensions::deserializeScalar(in, &num_bins_x_); - eigen_extensions::deserializeScalar(in, &num_bins_y_); - eigen_extensions::deserializeScalar(in, &training_samples_); + if(ascii) + { + eigen_extensions::deserializeScalarASCII(in, &width_); + eigen_extensions::deserializeScalarASCII(in, &height_); + eigen_extensions::deserializeScalarASCII(in, &bin_width_); + eigen_extensions::deserializeScalarASCII(in, &bin_height_); + eigen_extensions::deserializeScalarASCII(in, &bin_depth_); + eigen_extensions::deserializeScalarASCII(in, &num_bins_x_); + eigen_extensions::deserializeScalarASCII(in, &num_bins_y_); + eigen_extensions::deserializeScalarASCII(in, &training_samples_); + } + else + { + eigen_extensions::deserializeScalar(in, &width_); + eigen_extensions::deserializeScalar(in, &height_); + eigen_extensions::deserializeScalar(in, &bin_width_); + eigen_extensions::deserializeScalar(in, &bin_height_); + eigen_extensions::deserializeScalar(in, &bin_depth_); + eigen_extensions::deserializeScalar(in, &num_bins_x_); + eigen_extensions::deserializeScalar(in, &num_bins_y_); + eigen_extensions::deserializeScalar(in, &training_samples_); + } UINFO("Distortion Model: width=%d", width_); UINFO("Distortion Model: height=%d", height_); UINFO("Distortion Model: bin_width=%d", bin_width_); @@ -336,7 +394,7 @@ namespace clams for(size_t x = 0; x < frustums_[y].size(); ++x) { UDEBUG("Distortion Model: Frustum[%d][%d]", y, x); frustums_[y][x] = new DiscreteFrustum; - frustums_[y][x]->deserialize(in); + frustums_[y][x]->deserialize(in, ascii); } } UDEBUG(""); diff --git a/corelib/src/clams/eigen_extensions/eigen_extensions.h b/corelib/src/clams/eigen_extensions/eigen_extensions.h index a960aa00..eb38e565 100644 --- a/corelib/src/clams/eigen_extensions/eigen_extensions.h +++ b/corelib/src/clams/eigen_extensions/eigen_extensions.h @@ -28,25 +28,7 @@ namespace eigen_extensions { double var = total / (double)vec.rows(); return sqrt(var); } - - template - void save(const Eigen::Matrix& mat, const std::string& filename); - template - void load(const std::string& filename, Eigen::Matrix* mat); - - template - void save(const Eigen::SparseMatrix& mat, const std::string& filename); - - template - void load(const std::string& filename, Eigen::SparseMatrix* mat); - - template - void saveASCII(const Eigen::Matrix& mat, const std::string& filename); - - template - void loadASCII(const std::string& filename, Eigen::Matrix* mat); - template void serialize(const Eigen::Matrix& mat, std::ostream& strm); @@ -60,15 +42,6 @@ namespace eigen_extensions { void deserializeASCII(std::istream& strm, Eigen::Matrix* mat); - // -- SparseMatrix serialization. - - template - void serialize(const Eigen::SparseMatrix& mat, std::ostream& strm); - - template - void deserialize(std::istream& strm, Eigen::SparseMatrix* mat); - - // -- Scalar serialization // TODO: Can you name these {de,}serialize() and still have the right // functions get called when serializing matrices? @@ -78,6 +51,12 @@ namespace eigen_extensions { template void deserializeScalar(std::istream& strm, T* val); + template + void serializeScalarASCII(T val, std::ostream& strm); + + template + void deserializeScalarASCII(std::istream& strm, T* val); + /************************************************************ * Template implementations @@ -111,133 +90,7 @@ namespace eigen_extensions { *mat = Eigen::Map< Eigen::Matrix >(buf, rows, cols); free(buf); } -/* - template - void save(const Eigen::Matrix& mat, const std::string& filename) - { - assert(filename.size() > 3); - if(filename.substr(filename.size() - 3, 3).compare(".gz") == 0) { - ogzstream file(filename.c_str()); - assert(file); - serialize(mat, file); - file.close(); - } - else { - assert(boost::filesystem::extension(filename).compare(".eig") == 0); - std::ofstream file(filename.c_str()); - assert(file); - serialize(mat, file); - file.close(); - } - } - template - void load(const std::string& filename, Eigen::Matrix* mat) - { - assert(filename.size() > 3); - if(filename.substr(filename.size() - 3, 3).compare(".gz") == 0) { - igzstream file(filename.c_str()); - assert(file); - deserialize(file, mat); - file.close(); - } - else { - assert(boost::filesystem::extension(filename).compare(".eig") == 0); - std::ifstream file(filename.c_str()); - assert(file); - deserialize(file, mat); - file.close(); - } - } -*/ - template - void serialize(const Eigen::SparseMatrix& mat, std::ostream& strm) - { - int bytes = sizeof(ScalarType); - int type = Options; - int outer = mat.outerSize(); - int inner = mat.innerSize(); - int nnz = mat.nonZeros(); - strm.write((char*)&bytes, sizeof(int)); - strm.write((char*)&type, sizeof(int)); - strm.write((char*)&outer, sizeof(int)); - strm.write((char*)&inner, sizeof(int)); - strm.write((char*)&nnz, sizeof(int)); - - typedef typename Eigen::SparseMatrix::InnerIterator InnerIterator; - for(IndexType i = 0; i < mat.outerSize(); ++i) { - int num = 0; - for(InnerIterator it(mat, i); it; ++it) - ++num; - strm.write((const char*)&num, sizeof(num)); - - for(InnerIterator it(mat, i); it; ++it) { - int idx = it.index(); - ScalarType buf = it.value(); - strm.write((const char*)&idx, sizeof(idx)); - strm.write((const char*)&buf, sizeof(buf)); - } - } - } - - template - void deserialize(std::istream& strm, Eigen::SparseMatrix* mat) - { - int bytes; - int options; - int outer; - int inner; - int nnz; - strm.read((char*)&bytes, sizeof(int)); - strm.read((char*)&options, sizeof(int)); - strm.read((char*)&outer, sizeof(int)); - strm.read((char*)&inner, sizeof(int)); - strm.read((char*)&nnz, sizeof(int)); - assert(bytes == sizeof(ScalarType)); - assert(options == Options); - - if(mat->IsRowMajor) - mat->resize(outer, inner); - else - mat->resize(inner, outer); - - mat->reserve(nnz); - ScalarType buf; - for(int i = 0; i < mat->outerSize(); ++i) { - mat->startVec(i); - int num; - strm.read((char*)&num, sizeof(int)); - int idx; - for(int j = 0; j < num; ++j) { - strm.read((char*)&idx, sizeof(idx)); - strm.read((char*)&buf, sizeof(buf)); - mat->insertBackByOuterInner(i, idx) = buf; - } - } - mat->finalize(); - } -/* - template - void save(const Eigen::SparseMatrix& mat, const std::string& filename) - { - assert(boost::filesystem::extension(filename).compare(".eig") == 0); - std::ofstream file(filename.c_str()); - assert(file); - serialize(mat, file); - file.close(); - } - - template - void load(const std::string& filename, Eigen::SparseMatrix* mat) - { - assert(filename.size() > 3); - assert(boost::filesystem::extension(filename).compare(".eig") == 0); - std::ifstream file(filename.c_str()); - assert(file); - deserialize(file, mat); - file.close(); - } - */ template void serializeASCII(const Eigen::Matrix& mat, std::ostream& strm) { @@ -271,30 +124,6 @@ namespace eigen_extensions { } } - template - void saveASCII(const Eigen::Matrix& mat, const std::string& filename) - { - assert(filename.substr(filename.size() - 8).compare(".eig.txt") == 0); - std::ofstream file; - file.open(filename.c_str()); - assert(file); - serializeASCII(mat, file); - file.close(); - } - - template - void loadASCII(const std::string& filename, Eigen::Matrix* mat) - { - assert(filename.substr(filename.size() - 8).compare(".eig.txt") == 0); - std::ifstream file; - file.open(filename.c_str()); - if(!file) - std::cerr << "File " << filename << " could not be opened. Dying badly." << std::endl; - assert(file); - deserializeASCII(file, mat); - file.close(); - } - template void serializeScalar(T val, std::ostream& strm) { @@ -306,6 +135,25 @@ namespace eigen_extensions { { strm.read((char*)val, sizeof(T)); } + + template + void serializeScalarASCII(T val, std::ostream& strm) + { + int old_precision = strm.precision(); + strm.precision(16); + strm << "% " << val << std::endl; + strm.precision(old_precision); + } + + template + void deserializeScalarASCII(std::istream& strm, T* val) + { + std::string line; + while(line.length() == 0) getline(strm, line); + assert(line[0] == '%'); + std::istringstream iss(line.substr(1)); + iss >> *val; + } } diff --git a/guilib/src/DepthCalibrationDialog.cpp b/guilib/src/DepthCalibrationDialog.cpp index 222cfbf7..f9fb589c 100644 --- a/guilib/src/DepthCalibrationDialog.cpp +++ b/guilib/src/DepthCalibrationDialog.cpp @@ -152,7 +152,7 @@ void DepthCalibrationDialog::saveModel() { if(_model && _model->getTrainingSamples()) { - QString path = QFileDialog::getSaveFileName(this, tr("Save distortion model to ..."), _workingDirectory+QDir::separator()+"distortion_model.bin", tr("Distortion model (*.bin)")); + QString path = QFileDialog::getSaveFileName(this, tr("Save distortion model to ..."), _workingDirectory+QDir::separator()+"distortion_model.bin", tr("Distortion model (*.bin *.txt)")); if(!path.isEmpty()) { // @@ -163,7 +163,7 @@ void DepthCalibrationDialog::saveModel() if(!results.empty()) { - QString name = QString(path).replace(".bin", ".png", Qt::CaseInsensitive); + QString name = QString(path).replace(".bin", ".png", Qt::CaseInsensitive).replace(".txt", ".png", Qt::CaseInsensitive); cv::imwrite(name.toStdString(), results); QDesktopServices::openUrl(QUrl::fromLocalFile(name)); } diff --git a/guilib/src/ExportCloudsDialog.cpp b/guilib/src/ExportCloudsDialog.cpp index ae1710a0..1bfab91e 100644 --- a/guilib/src/ExportCloudsDialog.cpp +++ b/guilib/src/ExportCloudsDialog.cpp @@ -361,7 +361,7 @@ void ExportCloudsDialog::selectDistortionModel() { dir = _workingDirectory; } - QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Distortion model (*.bin)")); + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Distortion model (*.bin *.txt)")); if(path.size()) { _ui->lineEdit_distortionModel->setText(path); diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 0fd12a33..409309d5 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -3028,7 +3028,7 @@ void PreferencesDialog::selectSourceDistortionModel() { dir = getWorkingDirectory(); } - QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_source_distortionModel->text(), tr("Distortion model (*.bin)")); + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), _ui->lineEdit_source_distortionModel->text(), tr("Distortion model (*.bin *.txt)")); if(!path.isEmpty()) { _ui->lineEdit_source_distortionModel->setText(path);