i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 01:20:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

Integration of CLAMS depth calibration #114

Browse Source
This commit is contained in:
matlabbe
2016-09-19 14:01:06 -04:00
parent 861cc437f4
commit 3434b95d68
30 changed files with 2867 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files

344
corelib/src/clams/discrete_depth_distortion_model.cpp Normal file
View File

@@ -0,0 +1,344 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
#include "eigen_extensions/eigen_extensions.h"
using namespace std;
using namespace Eigen;
namespace clams
{
DiscreteFrustum::DiscreteFrustum(int smoothing, double bin_depth) :
max_dist_(10),
bin_depth_(bin_depth)
{
num_bins_ = ceil(max_dist_ / bin_depth_);
counts_ = VectorXf::Ones(num_bins_) * smoothing;
total_numerators_ = VectorXf::Ones(num_bins_) * smoothing;
total_denominators_ = VectorXf::Ones(num_bins_) * smoothing;
multipliers_ = VectorXf::Ones(num_bins_);
}
void DiscreteFrustum::addExample(double ground_truth, double measurement)
{
double mult = ground_truth / measurement;
if(mult > MAX_MULT || mult < MIN_MULT)
return;
int idx = min(num_bins_ - 1, (int)floor(measurement / bin_depth_));
UASSERT(idx >= 0);
total_numerators_(idx) += ground_truth * ground_truth;
total_denominators_(idx) += ground_truth * measurement;
++counts_(idx);
multipliers_(idx) = total_numerators_(idx) / total_denominators_(idx);
}
inline int DiscreteFrustum::index(double z) const
{
return min(num_bins_ - 1, (int)floor(z / bin_depth_));
}
inline void DiscreteFrustum::undistort(double* z) const
{
*z *= multipliers_.coeffRef(index(*z));
}
void DiscreteFrustum::interpolatedUndistort(double* z) const
{
int idx = index(*z);
double start = bin_depth_ * idx;
int idx1;
if(*z - start < bin_depth_ / 2)
idx1 = idx;
else
idx1 = idx + 1;
int idx0 = idx1 - 1;
if(idx0 < 0 || idx1 >= num_bins_ || counts_(idx0) < 50 || counts_(idx1) < 50) {
undistort(z);
return;
}
double z0 = (idx0 + 1) * bin_depth_ - bin_depth_ * 0.5;
double coeff1 = (*z - z0) / bin_depth_;
double coeff0 = 1.0 - coeff1;
double mult = coeff0 * multipliers_.coeffRef(idx0) + coeff1 * multipliers_.coeffRef(idx1);
*z *= mult;
}
void DiscreteFrustum::serialize(std::ostream& out) 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);
}
void DiscreteFrustum::deserialize(std::istream& in)
{
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_);
}
DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(const DiscreteDepthDistortionModel& other)
{
*this = other;
}
DiscreteDepthDistortionModel& DiscreteDepthDistortionModel::operator=(const DiscreteDepthDistortionModel& other)
{
width_ = other.width_;
height_ = other.height_;
bin_width_ = other.bin_width_;
bin_height_ = other.bin_height_;
bin_depth_ = other.bin_depth_;
num_bins_x_ = other.num_bins_x_;
num_bins_y_ = other.num_bins_y_;
training_samples_ = other.training_samples_;
frustums_ = other.frustums_;
for(size_t i = 0; i < frustums_.size(); ++i)
for(size_t j = 0; j < frustums_[i].size(); ++j)
frustums_[i][j] = new DiscreteFrustum(*other.frustums_[i][j]);
return *this;
}
DiscreteDepthDistortionModel::DiscreteDepthDistortionModel(int width, int height,
int bin_width, int bin_height,
double bin_depth,
int smoothing) :
width_(width),
height_(height),
bin_width_(bin_width),
bin_height_(bin_height),
bin_depth_(bin_depth)
{
UASSERT(width_ % bin_width_ == 0);
UASSERT(height_ % bin_height_ == 0);
num_bins_x_ = width_ / bin_width_;
num_bins_y_ = height_ / bin_height_;
frustums_.resize(num_bins_y_);
for(size_t i = 0; i < frustums_.size(); ++i) {
frustums_[i].resize(num_bins_x_, NULL);
for(size_t j = 0; j < frustums_[i].size(); ++j)
frustums_[i][j] = new DiscreteFrustum(smoothing, bin_depth);
}
training_samples_ = 0;
}
void DiscreteDepthDistortionModel::deleteFrustums()
{
for(size_t y = 0; y < frustums_.size(); ++y)
for(size_t x = 0; x < frustums_[y].size(); ++x)
if(frustums_[y][x])
delete frustums_[y][x];
training_samples_ = 0;
}
DiscreteDepthDistortionModel::~DiscreteDepthDistortionModel()
{
deleteFrustums();
}
void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
{
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float & z = depth.at<float>(v, u);
if(z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
z = zf;
}
}
}
else
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);
z = zf*1000;
}
}
}
}
void DiscreteDepthDistortionModel::addExample(int v, int u, double ground_truth, double measurement)
{
frustum(v, u).addExample(ground_truth, measurement);
}
size_t DiscreteDepthDistortionModel::accumulate(const cv::Mat& ground_truth,
const cv::Mat& measurement)
{
UASSERT(width_ == ground_truth.cols);
UASSERT(height_ == ground_truth.rows);
UASSERT(width_ == measurement.cols);
UASSERT(height_ == measurement.rows);
UASSERT(ground_truth.type() == CV_16UC1 || ground_truth.type() == CV_32FC1);
UASSERT(measurement.type() == CV_16UC1 || measurement.type() == CV_32FC1);
bool isGroundTruthInMM = ground_truth.type()==CV_16UC1;
bool isMeasurementInMM = measurement.type()==CV_16UC1;
size_t num_training_examples = 0;
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
float gt = isGroundTruthInMM?float(ground_truth.at<unsigned short>(v,u))*0.001:ground_truth.at<float>(v,u);
if(gt == 0)
continue;
float meas = isMeasurementInMM?float(measurement.at<unsigned short>(v,u))*0.001:measurement.at<float>(v,u);
if(meas == 0)
continue;
UScopeMutex sm(mutex_);
frustum(v, u).addExample(gt, meas);
++num_training_examples;
}
}
training_samples_ += num_training_examples;
return num_training_examples;
}
void DiscreteDepthDistortionModel::load(const std::string& path)
{
ifstream f;
f.open(path.c_str());
if(!f.is_open()) {
cerr << "Failed to open " << path << endl;
assert(f.is_open());
}
deserialize(f);
f.close();
}
void DiscreteDepthDistortionModel::save(const std::string& path) const
{
ofstream f;
f.open(path.c_str());
if(!f.is_open()) {
cerr << "Failed to open " << path << endl;
assert(f.is_open());
}
serialize(f);
f.close();
}
void DiscreteDepthDistortionModel::serialize(std::ostream& out) 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);
for(int y = 0; y < num_bins_y_; ++y)
for(int x = 0; x < num_bins_x_; ++x)
frustums_[y][x]->serialize(out);
}
void DiscreteDepthDistortionModel::deserialize(std::istream& in)
{
string buf;
getline(in, buf);
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_);
deleteFrustums();
frustums_.resize(num_bins_y_);
for(size_t y = 0; y < frustums_.size(); ++y) {
frustums_[y].resize(num_bins_x_, NULL);
for(size_t x = 0; x < frustums_[y].size(); ++x) {
frustums_[y][x] = new DiscreteFrustum;
frustums_[y][x]->deserialize(in);
}
}
}
DiscreteFrustum& DiscreteDepthDistortionModel::frustum(int y, int x)
{
UASSERT(x >= 0 && x < width_);
UASSERT(y >= 0 && y < height_);
int xidx = x / bin_width_;
int yidx = y / bin_height_;
return (*frustums_[yidx][xidx]);
}
const DiscreteFrustum& DiscreteDepthDistortionModel::frustum(int y, int x) const
{
UASSERT(x >= 0 && x < width_);
UASSERT(y >= 0 && y < height_);
int xidx = x / bin_width_;
int yidx = y / bin_height_;
return (*frustums_[yidx][xidx]);
}
} // namespace clams

207
corelib/src/clams/discrete_depth_distortion_model_helpers.cpp Normal file
View File

@@ -0,0 +1,207 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include <rtabmap/core/clams/discrete_depth_distortion_model.h>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/utilite/UDirectory.h>
using namespace std;
using namespace Eigen;
namespace clams
{
cv::Mat DiscreteDepthDistortionModel::visualize(const std::string& dir) const
{
bool writeFiles = !dir.empty() && UDirectory::exists(dir);
const DiscreteFrustum& reference_frustum = *frustums_[0][0];
int num_layers = reference_frustum.num_bins_;
// -- Set up for combined imagery.
int horiz_divider = 10;
int vert_divider = 20;
cv::Mat3b overview(cv::Size(width_ * 2 + horiz_divider, height_ * num_layers + vert_divider * (num_layers + 2)), cv::Vec3b(0, 0, 0));
vector<int> pub_layers;
for(int i = 0; i < num_layers; ++i)
pub_layers.push_back(i);
// pub_layers.push_back(1);
// pub_layers.push_back(2);
// pub_layers.push_back(3);
cv::Mat3b pub(cv::Size(width_, height_ * pub_layers.size() + vert_divider * (pub_layers.size() + 2)), cv::Vec3b(255, 255, 255));
for(int i = 0; i < num_layers; ++i) {
// -- Determine the path to save the image for this layer.
char buffer[50];
float mindepth = reference_frustum.bin_depth_ * i;
float maxdepth = reference_frustum.bin_depth_ * (i + 1);
sprintf(buffer, "%05.2f-%05.2f", mindepth, maxdepth);
ostringstream oss;
oss << dir << "/multipliers_" << buffer << ".png";
// -- Compute the multipliers visualization for this layer.
// Multiplier of 1 is black, >1 is red, <1 is blue. Think redshift.
cv::Mat3b mult(cv::Size(width_, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < mult.rows; ++y) {
for(int x = 0; x < mult.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
float val = frustum.multipliers_(i);
if(val > 1)
mult(y, x)[2] = min(255., 255 * (val - 1.0) / 0.25);
if(val < 1)
mult(y, x)[0] = min(255., 255 * (1.0 - val) / 0.25);
}
}
if(writeFiles)
{
cv::imwrite(oss.str(), mult);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Compute the counts visualization for this layer.
// 0 is black, 100 is white.
cv::Mat3b count(cv::Size(width_, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < count.rows; ++y) {
for(int x = 0; x < count.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
uchar val = min(255., (double)(255 * frustum.counts_(i) / 100));
count(y, x)[0] = val;
count(y, x)[1] = val;
count(y, x)[2] = val;
}
}
oss.str("");
oss << dir << "/counts_" << buffer << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), count);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Make images showing the two, side-by-side.
cv::Mat3b combined(cv::Size(width_ * 2 + horiz_divider, height_), cv::Vec3b(0, 0, 0));
for(int y = 0; y < combined.rows; ++y) {
for(int x = 0; x < combined.cols; ++x) {
if(x < count.cols)
combined(y, x) = count(y, x);
else if(x > count.cols + horiz_divider)
combined(y, x) = mult(y, x - count.cols - horiz_divider);
}
}
oss.str("");
oss << dir << "/combined_" << buffer << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), combined);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Append to the overview image.
for(int y = 0; y < combined.rows; ++y)
for(int x = 0; x < combined.cols; ++x)
overview(y + i * (combined.rows + vert_divider) + vert_divider, x) = combined(y, x);
// -- Compute the publication multipliers visualization for this layer.
// Multiplier of 1 is white, >1 is red, <1 is blue. Think redshift.
cv::Mat3b pubmult(cv::Size(width_, height_), cv::Vec3b(255, 255, 255));
for(int y = 0; y < pubmult.rows; ++y) {
for(int x = 0; x < pubmult.cols; ++x) {
const DiscreteFrustum& frustum = *frustums_[y / bin_height_][x / bin_width_];
float val = frustum.multipliers_(i);
if(val > 1) {
pubmult(y, x)[0] = 255 - min(255., 255 * (val - 1.0) / 0.1);
pubmult(y, x)[1] = 255 - min(255., 255 * (val - 1.0) / 0.1);
}
if(val < 1) {
pubmult(y, x)[1] = 255 - min(255., 255 * (1.0 - val) / 0.1);
pubmult(y, x)[2] = 255 - min(255., 255 * (1.0 - val) / 0.1);
}
}
}
// -- Append to publication image.
for(size_t j = 0; j < pub_layers.size(); ++j)
if(pub_layers[j] == i)
for(int y = 0; y < pubmult.rows; ++y)
for(int x = 0; x < pubmult.cols; ++x)
pub(y + j * (pubmult.rows + vert_divider) + vert_divider, x) = pubmult(y, x);
}
// -- Add a white bar at the top and bottom for reference.
for(int y = 0; y < overview.rows; ++y)
if(y < vert_divider || y > overview.rows - vert_divider)
for(int x = 0; x < overview.cols; ++x)
overview(y, x) = cv::Vec3b(255, 255, 255);
// -- Save overview image.
ostringstream oss;
oss << dir << "/overview.png";
if(writeFiles)
{
cv::imwrite(oss.str(), overview);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Save a small version for easy loading.
cv::Mat3b overview_scaled;
cv::resize(overview, overview_scaled, cv::Size(), 0.2, 0.2, cv::INTER_CUBIC);
oss.str("");
oss << dir << "/overview_scaled.png";
if(writeFiles)
{
cv::imwrite(oss.str(), overview_scaled);
UINFO("Written \"%s\"", oss.str().c_str());
}
// -- Save publication image.
oss.str("");
oss << dir << "/pub";
// for(size_t i = 0; i < pub_layers.size(); ++i)
// oss << "-" << setw(2) << setfill('0') << pub_layers[i];
oss << ".png";
if(writeFiles)
{
cv::imwrite(oss.str(), pub);
UINFO("Written \"%s\"", oss.str().c_str());
}
UASSERT(overview.rows == pub.rows);
cv::Mat3b targetImage(overview.rows, overview.cols/2 + pub.cols);
cv::Mat roiA(targetImage, cv::Rect( 0, 0, overview.cols/2, overview.rows ));
cv::Mat(overview, cv::Rect( 0, 0, overview.cols/2, overview.rows )).copyTo(roiA);
cv::Mat roiB( targetImage, cvRect( overview.cols/2, 0, pub.cols, pub.rows ) );
pub.copyTo(roiB);
return targetImage;
}
} // namespace clams

312
corelib/src/clams/eigen_extensions/eigen_extensions.h Normal file
View File

@@ -0,0 +1,312 @@
#ifndef EIGEN_EXTENSIONS_H
#define EIGEN_EXTENSIONS_H
#include <Eigen/Eigen>
//#define EIGEN_YES_I_KNOW_SPARSE_MODULE_IS_NOT_STABLE_YET
#include <Eigen/Sparse>
//#include <boost/filesystem.hpp>
#include <stdint.h>
#include <fstream>
#include <iostream>
//#include <gzstream/gzstream.h>
namespace eigen_extensions {
inline void stdToEig(const std::vector<double>& std, Eigen::VectorXd* eig)
{
eig->resize(std.size());
for(size_t i = 0; i < std.size(); ++i)
eig->coeffRef(i) = std[i];
}
inline double stdev(const Eigen::VectorXd& vec)
{
double mean = vec.sum() / (double)vec.rows();
double total = 0;
for(int i = 0; i < vec.rows(); ++i)
total += (vec.coeffRef(i) - mean) * (vec.coeffRef(i) - mean);
double var = total / (double)vec.rows();
return sqrt(var);
}
template<class S, int T, int U>
void save(const Eigen::Matrix<S, T, U>& mat, const std::string& filename);
template<class S, int T, int U>
void load(const std::string& filename, Eigen::Matrix<S, T, U>* mat);
template<class ScalarType, int Options, class IndexType>
void save(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, const std::string& filename);
template<class ScalarType, int Options, class IndexType>
void load(const std::string& filename, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat);
template<class S, int T, int U>
void saveASCII(const Eigen::Matrix<S, T, U>& mat, const std::string& filename);
template<class S, int T, int U>
void loadASCII(const std::string& filename, Eigen::Matrix<S, T, U>* mat);
template<class S, int T, int U>
void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
template<class S, int T, int U>
void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
template<class S, int T, int U>
void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm);
template<class S, int T, int U>
void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat);
// -- SparseMatrix serialization.
template<class ScalarType, int Options, class IndexType>
void serialize(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& mat, std::ostream& strm);
template<class ScalarType, int Options, class IndexType>
void deserialize(std::istream& strm, Eigen::SparseMatrix<ScalarType, Options, IndexType>* mat);
// -- Scalar serialization
// TODO: Can you name these {de,}serialize() and still have the right
// functions get called when serializing matrices?
template<class T>
void serializeScalar(T val, std::ostream& strm);
template<class T>
void deserializeScalar(std::istream& strm, T* val);
/************************************************************
* Template implementations
************************************************************/
template<class S, int T, int U>
void serialize(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
{
int bytes = sizeof(S);
int rows = mat.rows();
int cols = mat.cols();
strm.write((char*)&bytes, sizeof(int));
strm.write((char*)&rows, sizeof(int));
strm.write((char*)&cols, sizeof(int));
strm.write((const char*)mat.data(), sizeof(S) * rows * cols);
}
template<class S, int T, int U>
void deserialize(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
{
int bytes;
int rows;
int cols;
strm.read((char*)&bytes, sizeof(int));
strm.read((char*)&rows, sizeof(int));
strm.read((char*)&cols, sizeof(int));
assert(bytes == sizeof(S));
S *buf = (S*) malloc(sizeof(S) * rows * cols);
strm.read((char*)buf, sizeof(S) * rows * cols);
*mat = Eigen::Map< Eigen::Matrix<S, T, U> >(buf, rows, cols);
free(buf);
}
/*
template<class S, int T, int U>
void save(const Eigen::Matrix<S, T, U>& 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<class S, int T, int U>
void load(const std::string& filename, Eigen::Matrix<S, T, U>* 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<class ScalarType, int Options, class IndexType>
void serialize(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& 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<ScalarType, Options, IndexType>::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<class ScalarType, int Options, class IndexType>
void deserialize(std::istream& strm, Eigen::SparseMatrix<ScalarType, Options, IndexType>* 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<class ScalarType, int Options, class IndexType>
void save(const Eigen::SparseMatrix<ScalarType, Options, IndexType>& 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<class ScalarType, int Options, class IndexType>
void load(const std::string& filename, Eigen::SparseMatrix<ScalarType, Options, IndexType>* 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<class S, int T, int U>
void serializeASCII(const Eigen::Matrix<S, T, U>& mat, std::ostream& strm)
{
int old_precision = strm.precision();
strm.precision(16);
strm << "% " << mat.rows() << " " << mat.cols() << std::endl;
strm << mat << std::endl;
strm.precision(old_precision);
}
template<class S, int T, int U>
void deserializeASCII(std::istream& strm, Eigen::Matrix<S, T, U>* mat)
{
// -- Read the header.
std::string line;
while(line.length() == 0) getline(strm, line);
assert(line[0] == '%');
std::istringstream iss(line.substr(1));
int rows;
int cols;
iss >> rows;
iss >> cols;
// -- Read in the data.
*mat = Eigen::Matrix<S, T, U>(rows, cols);
for(int y = 0; y < rows; ++y) {
getline(strm, line);
std::istringstream iss(line);
for(int x = 0; x < cols; ++x) {
iss >> mat->coeffRef(y, x);
}
}
}
template<class S, int T, int U>
void saveASCII(const Eigen::Matrix<S, T, U>& 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<class S, int T, int U>
void loadASCII(const std::string& filename, Eigen::Matrix<S, T, U>* 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<class T>
void serializeScalar(T val, std::ostream& strm)
{
strm.write((char*)&val, sizeof(T));
}
template<class T>
void deserializeScalar(std::istream& strm, T* val)
{
strm.read((char*)val, sizeof(T));
}
}
#endif // EIGEN_EXTENSIONS_H

241
corelib/src/clams/frame_projector.cpp Normal file
View File

@@ -0,0 +1,241 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
#include <rtabmap/core/util3d_transforms.h>
#include <rtabmap/core/util3d.h>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
using namespace std;
using namespace Eigen;
namespace clams
{
FrameProjector::FrameProjector(const rtabmap::CameraModel & model) :
model_(model)
{
UASSERT(model.isValidForReprojection());
}
// pcd is in /map frame
FrameProjector::RangeIndex FrameProjector::cloudToRangeIndex(const pcl::PointCloud<pcl::PointXYZ>::Ptr & pcd) const
{
int height = model_.imageHeight();
int width = model_.imageWidth();
RangeIndex ind;
if((int)ind.size() != height)
ind.resize(height);
for(size_t y = 0; y < ind.size(); ++y)
if((int)ind[y].size() != width)
ind[y].resize(width);
for(size_t y = 0; y < ind.size(); ++y) {
for(size_t x = 0; x < ind[y].size(); ++x) {
ind[y][x].clear();
ind[y][x].reserve(10);
}
}
rtabmap::Transform t = model_.localTransform().inverse();
ProjectivePoint ppt;
for(size_t i = 0; i < pcd->size(); ++i) {
if(!isFinite(pcd->at(i)))
continue;
ppt = reproject(rtabmap::util3d::transformPoint(pcd->at(i), t));
if(ppt.z_ == 0 || !(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < width && ppt.v_ < height))
continue;
ind[ppt.v_][ppt.u_].push_back(ppt.z_);
}
return ind;
}
pcl::PointXYZ FrameProjector::project(const ProjectivePoint& ppt) const
{
UASSERT(ppt.u_ >= 0 && ppt.v_ >= 0 && ppt.u_ < model_.imageWidth() && ppt.v_ < model_.imageHeight());
pcl::PointXYZ pt;
model_.project(ppt.u_, ppt.v_, ppt.z_, pt.x, pt.y, pt.z);
return pt;
}
ProjectivePoint FrameProjector::reproject(const pcl::PointXYZ & pt) const
{
UASSERT(isFinite(pt));
ProjectivePoint ppt;
if(pt.z > 0)
{
model_.reproject(pt.x, pt.y, pt.z, ppt.u_, ppt.v_);
ppt.z_ = pt.z;
}
return ppt;
}
cv::Mat FrameProjector::estimateMapDepth(
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
const rtabmap::Transform & transform,
const cv::Mat& measurement,
double coneRadius,
double coneStdevThresh) const
{
cv::Mat estimate = cv::Mat::zeros(measurement.size(), CV_32FC1);
// -- Get the depth index.
pcl::PointCloud<pcl::PointXYZ>::Ptr transformed = rtabmap::util3d::transformPointCloud(map, transform);
RangeIndex rindex = cloudToRangeIndex(transformed);
// -- Compute the edge-of-map mask.
cv::Mat naive_mapframe = rtabmap::util3d::projectCloudToCamera(model_.imageSize(), model_.K(), transformed, model_.localTransform());
const cv::Mat& measurement_depth = measurement;
const cv::Mat& naive_mapdepth = naive_mapframe;
cv::Mat1b mask(measurement_depth.rows, measurement_depth.cols);
mask = 0;
for(int y = 0; y < mask.rows; ++y)
for(int x = 0; x < mask.cols; ++x)
if(naive_mapdepth.at<float>(y, x) != 0)
mask(y, x) = 255;
cv::dilate(mask, mask, cv::Mat(), cv::Point(-1, -1), 4);
cv::erode(mask, mask, cv::Mat(), cv::Point(-1, -1), 15);
bool isInMM = measurement_depth.type() == CV_16UC1;
// -- Main loop: for all points in the image...
ProjectivePoint ppt;
for(ppt.v_ = 0; ppt.v_ < measurement_depth.rows; ++ppt.v_) {
for(ppt.u_ = 0; ppt.u_ < measurement_depth.cols; ++ppt.u_) {
float value = isInMM?float(measurement_depth.at<unsigned short>(ppt.v_, ppt.u_))*0.001f:measurement_depth.at<float>(ppt.v_, ppt.u_);
// -- Reject points with no data.
if(value == 0)
continue;
if(naive_mapdepth.at<float>(ppt.v_, ppt.u_) == 0)
continue;
// -- Reject points on the edge of the map.
if(mask(ppt.v_, ppt.u_) == 0)
continue;
// -- Find nearby points in the cone to get a good estimate of the map depth.
double mean = 0;
double stdev = 0;
//double stdev_thresh = numeric_limits<double>::max();
bool valid = coneFit(
naive_mapdepth.size(),
rindex,
ppt.u_,
ppt.v_,
coneRadius,
value,
&mean,
&stdev);
if(!valid)
continue;
if(stdev > coneStdevThresh)
continue;
estimate.at<float>(ppt.v_, ppt.u_) = mean;
}
}
return estimate;
}
bool FrameProjector::coneFit(const cv::Size& imageSize, const RangeIndex& rindex,
int uc, int vc, double radius, double measurement_depth,
double* mean, double* stdev) const
{
pcl::PointXYZ pt_center, pt_ul, pt_lr;
ProjectivePoint ppt, ppt_ul, ppt_lr;
ppt.u_ = uc;
ppt.v_ = vc;
ppt.z_ = measurement_depth;
pt_center = project(ppt);
pt_ul = pt_center;
pt_lr = pt_center;
pt_ul.x -= radius;
pt_ul.y -= radius;
pt_lr.x += radius;
pt_lr.y += radius;
ppt_ul = reproject(pt_ul);
ppt_lr = reproject(pt_lr);
if(ppt_ul.z_ == 0 || !(ppt_ul.u_ >= 0 && ppt_ul.v_ >= 0 && ppt_ul.u_ < imageSize.width && ppt_ul.v_ < imageSize.height))
return false;
if(ppt_lr.z_ == 0 || !(ppt_lr.u_ >= 0 && ppt_lr.v_ >= 0 && ppt_lr.u_ < imageSize.width && ppt_lr.v_ < imageSize.height))
return false;
int min_u = ppt_ul.u_;
int max_u = ppt_lr.u_;
int min_v = ppt_ul.v_;
int max_v = ppt_lr.v_;
*mean = 0;
double num = 0;
for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
const vector<double>& vals = rindex[ppt.v_][ppt.u_];
for(size_t i = 0; i < vals.size(); ++i) {
double mult = vals[i] / measurement_depth;
if(mult > MIN_MULT && mult < MAX_MULT) {
*mean += vals[i];
++num;
}
}
}
}
if(num == 0)
return false;
*mean /= num;
double var = 0;
for(ppt.u_ = min_u; ppt.u_ <= max_u; ++ppt.u_) {
for(ppt.v_ = min_v; ppt.v_ <= max_v; ++ppt.v_) {
const vector<double>& vals = rindex[ppt.v_][ppt.u_];
for(size_t i = 0; i < vals.size(); ++i) {
double mult = vals[i] / measurement_depth;
if(mult > MIN_MULT && mult < MAX_MULT)
var += (vals[i] - *mean) * (vals[i] - *mean);
}
}
}
var /= num;
*stdev = sqrt(var);
return true;
}
} // namespace clams

78
corelib/src/clams/slam_calibrator.cpp Normal file
View File

@@ -0,0 +1,78 @@
/*
Copyright (c) 2013, Alex Teichman and Stephen Miller (Stanford University)
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the <organization> nor the
names of its contributors may be used to endorse or promote products
derived from this software without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
RTAB-Map integration: Mathieu Labbe
*/
#include "rtabmap/core/clams/slam_calibrator.h"
#include "rtabmap/core/clams/frame_projector.h"
#include <rtabmap/utilite/ULogger.h>
using namespace std;
using namespace Eigen;
namespace clams
{
DiscreteDepthDistortionModel calibrate(
const std::map<int, rtabmap::SensorData> & sequence,
const std::map<int, rtabmap::Transform> & trajectory,
const pcl::PointCloud<pcl::PointXYZ>::Ptr & map,
double coneRadius,
double coneStdevThresh)
{
DiscreteDepthDistortionModel model;
if(!sequence.empty())
{
const cv::Size & imageSize = sequence.begin()->second.cameraModels()[0].imageSize();
model = DiscreteDepthDistortionModel(imageSize.width, imageSize.height);
// -- For all selected frames, accumulate training examples
// in the distortion model.
size_t counts;
#pragma omp parallel for
for(std::map<int, rtabmap::Transform>::const_iterator iter = trajectory.begin(); iter != trajectory.end(); ++iter)
{
size_t idx = iter->first;
std::map<int, rtabmap::SensorData>::const_iterator ster = sequence.find(idx);
if(ster!=sequence.end())
{
cv::Mat depthImage;
ster->second.uncompressDataConst(0, &depthImage);
cv::Mat mapDepth;
FrameProjector projector(ster->second.cameraModels()[0]);
mapDepth = projector.estimateMapDepth(map, iter->second.inverse(), depthImage, coneRadius, coneStdevThresh);
counts = model.accumulate(mapDepth, depthImage);
}
}
UINFO("counts=%d", (int)counts);
}
return model;
}
} // namespace clams