Skip to content

Commit

Permalink
Fixed some bugs and changed visualization to color images
Browse files Browse the repository at this point in the history
  • Loading branch information
@mtlazaro
mtlazaro committed Aug 4, 2015
1 parent 0c28dfa commit b847c50
Show file tree
Hide file tree
Showing 6 changed files with 55 additions and 50 deletions.
4 changes: 4 additions & 0 deletions src/thin_scan_matcher_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,6 +42,7 @@ int main(int argc, char **argv) {
bool publish_odom;
bool publish_tf;
double min_correspondences_ratio;
int iterations;

double local_map_clipping_range;
double local_map_clipping_translation_threshold;
Expand Down Expand Up @@ -70,6 +71,7 @@ int main(int argc, char **argv) {
n.param("publish_odom", publish_odom, true);
n.param("publish_tf", publish_tf, false);
n.param("inlier_distance", inlier_distance, 0.1);
n.param("iterations", iterations, 10);
n.param("frame_skip", frame_skip, 1);
n.param("bpr", bpr, 0.2);
n.param("min_correspondences_ratio", min_correspondences_ratio, 0.3);
Expand Down Expand Up @@ -103,12 +105,14 @@ int main(int argc, char **argv) {
std::cout << "_min_matching_range:=" << min_matching_range << std::endl;
std::cout << "_num_matching_beams:=" << num_matching_beams << std::endl;
std::cout << "_matching_fov:=" << matching_fov << std::endl;
std::cout << "_iterations:=" << iterations << std::endl;

fflush(stdout);

// Setting input parameters
message_handler.setFrameSkip(frame_skip);
tracker.setBpr(bpr);
tracker.setIterations(iterations);
tracker.setInlierDistance(inlier_distance);
tracker.setMinCorrespondencesRatio(min_correspondences_ratio);
tracker.setLocalMapClippingRange(local_map_clipping_range);
Expand Down
39 changes: 16 additions & 23 deletions src/tsm_core/cloud2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -94,11 +94,9 @@ namespace tsm{
model = sparse_model;
}

void Cloud2D::draw(UnsignedCharImage &img, bool draw_normals, Eigen::Isometry2f T,
bool draw_pose_origin) const {
std::vector<cv::Point2i> pt_to_draw;
std::vector<cv::Point2i> normal_to_draw;
int scale = 55;
void Cloud2D::draw(RGBImage &img, cv::Vec3b color, bool draw_normals, Eigen::Isometry2f T,
bool draw_pose_origin, float scale) const {
std::vector<cv::Point2i> normal_to_draw, pt_to_draw;
float max_value = std::numeric_limits<float>::min();

for(Cloud2D::const_iterator it = begin(); it != end(); ++it) {
Expand All @@ -118,44 +116,39 @@ namespace tsm{
pt_to_draw.push_back(cv::Point2i(static_cast<int>(pt.x()), static_cast<int>(pt.y())));
normal_to_draw.push_back(cv::Point2i(static_cast<int>(n.x()), static_cast<int>(n.y())));
}

float max_dest_size = std::max(img.rows, img.cols);
float img_size = std::max(max_value * scale, max_dest_size);
int img_size = std::max(max_value * scale, max_dest_size);

UnsignedCharImage tmp = cv::Mat::zeros(img_size, img_size, CV_8UC1);

RGBImage tmp = cv::Mat::zeros(img_size, img_size, CV_8UC3);
for(size_t i = 0; i < pt_to_draw.size(); ++i) {
cv::Point2i& pt = pt_to_draw[i];

pt.x += img_size * 0.5;
pt.y += img_size * 0.5;

cv::Point2i& normal = normal_to_draw[i];

if (draw_normals)
cv::line(
tmp,
cv::line(tmp,
pt,
pt + normal,
100
);

tmp.at<uchar>(pt.y, pt.x) = 255;
tmp.at<cv::Vec3b>(pt.y,pt.x) = color;
}

cv::circle(tmp,
cv::Point(
T.translation().x() * scale * 0.5 + img_size * 0.5,
T.translation().y() * scale * 0.5 + img_size * 0.5
),
cv::Point(T.translation().x() * scale * 0.5 + img_size * 0.5,
T.translation().y() * scale * 0.5 + img_size * 0.5),
3,
150,
cv::Scalar(color[0],color[1],color[2]),
CV_FILLED);

if (img.size().area() > 0) {
float max_cols = std::max(img.cols, tmp.cols);
cv::Rect roi = cv::Rect(
(tmp.cols - img.cols) * 0.5,
cv::Rect roi = cv::Rect((tmp.cols - img.cols) * 0.5,
(tmp.rows - img.rows) * 0.5,
img.cols,
img.rows);
Expand Down
4 changes: 2 additions & 2 deletions src/tsm_core/cloud2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,9 +68,9 @@ namespace tsm {
void voxelize(Cloud2D& model, float res);

//! draws a cloud, by applying the provided transformation, with normals and origin pose
void draw(UnsignedCharImage& img, bool draw_normals = false,
void draw(RGBImage& img, cv::Vec3b color, bool draw_normals = false,
Eigen::Isometry2f T = Eigen::Isometry2f::Identity(),
bool draw_pose_origin = false) const;
bool draw_pose_origin = false, float scale = 20) const;

virtual ~Cloud2D();
};
Expand Down
42 changes: 24 additions & 18 deletions src/tsm_core/correspondence_finder2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,15 @@
#include <stdexcept>

namespace tsm {

CorrespondenceFinder2D::CorrespondenceFinder2D(Projector2D* projector, Solver2D* solver) {
_correspondences.clear();
_projector = projector;
_solver = solver;
_max_squared_distance=0.3*0.3;
_min_normal_cos=cos(M_PI/8);
}

void CorrespondenceFinder2D::init() {
_correspondences.clear();
_indices_current.clear();
Expand Down Expand Up @@ -43,40 +52,37 @@ namespace tsm {
continue;

RichPoint2D pt_curr = (*_solver->current())[idx1];
pt_curr.transformInPlace(_solver->T());
RichPoint2D pt_ref = (*_solver->reference())[idx2];

if((pt_curr.point() - pt_ref.point()).squaredNorm() < 0.09 &&
pt_curr.normal().dot(pt_ref.normal()) > 0.8) {
if((pt_curr.point() - pt_ref.point()).squaredNorm() < _max_squared_distance &&
pt_curr.normal().dot(pt_ref.normal()) > _min_normal_cos) {
_correspondences.push_back(i);
}
}
}

void CorrespondenceFinder2D::drawCorrespondences(UnsignedCharImage& img) const {
UnsignedCharImage tmp;

_solver->current()->draw(tmp, true);
_solver->reference()->draw(tmp, true);

void CorrespondenceFinder2D::drawCorrespondences(RGBImage &img, Eigen::Isometry2f T, float scale) const {
_solver->current()->draw(img, cv::Vec3b(0,255,0), false, T, false, scale);
_solver->reference()->draw(img, cv::Vec3b(0,0,255), false, Eigen::Isometry2f::Identity(), false, scale);

T.linear()*=scale;
for (size_t i = 0; i < _correspondences.size(); ++i) {
int idx1 = _indices_current[_correspondences[i]];
int idx2 = _indices_reference[_correspondences[i]];

Eigen::Vector2f pt_curr = (*_solver->current())[idx1].point() * 10;
Eigen::Vector2f pt_ref = (*_solver->reference())[idx2].point() * 10;

float center_x = tmp.cols * 0.5;
float center_y = tmp.rows * 0.5;
Eigen::Vector2f pt_curr = T*(*_solver->current())[idx1].point() * 0.5;
Eigen::Vector2f pt_ref = T*(*_solver->reference())[idx2].point() * 0.5;

cv::line(
tmp,
float center_x = img.cols * 0.5;
float center_y = img.rows * 0.5;
cv::line(img,
cv::Point(pt_curr.x() + center_x, pt_curr.y() + center_y),
cv::Point(pt_ref.x() + center_x, pt_ref.y() + center_y),
255
cv::Scalar(255, 0, 0)
);
}

img = tmp;
}

}

15 changes: 9 additions & 6 deletions src/tsm_core/correspondence_finder2d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,18 +7,14 @@ namespace tsm {
class CorrespondenceFinder2D {
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
CorrespondenceFinder2D(Projector2D* projector = NULL, Solver2D* solver = NULL) {
_correspondences.clear();
_projector = projector;
_solver = solver;
}
CorrespondenceFinder2D(Projector2D* projector = NULL, Solver2D* solver = NULL);
~CorrespondenceFinder2D() {};
void compute();
void init();
inline const std::vector<int>& correspondences() const { return _correspondences; }
inline const IntVector& indicesCurrent() const { return _indices_current; }
inline const IntVector& indicesReference() const { return _indices_reference; }
void drawCorrespondences(UnsignedCharImage& img) const;
void drawCorrespondences(RGBImage& img, Eigen::Isometry2f T, float scale=20) const;

// setter & getter
inline void setProjector(const Projector2D* projector) {
Expand All @@ -27,12 +23,19 @@ namespace tsm {
inline void setSolver(const Solver2D* solver) {
_solver = solver;
}
inline float minNormalCos() const {return _min_normal_cos;}
inline void setMinNormalCos(float mnc) {_min_normal_cos=mnc;}
inline float maxSquaredDistance() const {return _max_squared_distance;}
inline void setMaxSquaredDistance(float msd) {_max_squared_distance=msd;}

private:
const Projector2D* _projector;
const Solver2D* _solver;
IntVector _indices_current, _indices_reference;
FloatVector _projected_current_ranges, _projected_reference_ranges;

float _max_squared_distance;
float _min_normal_cos;
std::vector<int> _correspondences;
};
}
1 change: 0 additions & 1 deletion src/tsm_core/projector2d.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,6 @@ namespace tsm {
if (p0.dot(n) > 0)
n = -n;

Eigen::Vector2f t(n.y(), -n.x());
model[i]._normal = n;
}
}
Expand Down

0 comments on commit b847c50

Please sign in to comment.