Skip to content

Commit

Permalink
fix warnings and apply clang-format to fixed files
Browse files Browse the repository at this point in the history
  • Loading branch information
Sascha Wirges committed Apr 13, 2017
1 parent 383e60e commit 85a911d
Show file tree
Hide file tree
Showing 5 changed files with 99 additions and 65 deletions.
33 changes: 19 additions & 14 deletions include/stargazer/DebugVisualizer.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,8 @@
namespace stargazer {

/**
* @brief This class is used for debugging. It contains several methods for visualizing points and landmarks.
* @brief This class is used for debugging. It contains several methods for visualizing points and
* landmarks.
*
*/
class DebugVisualizer {
Expand All @@ -35,13 +36,13 @@ class DebugVisualizer {
* @brief Constructor
*
*/
DebugVisualizer();
inline DebugVisualizer(){};

/**
* @brief Destructor
*
*/
~DebugVisualizer(){};
inline ~DebugVisualizer(){};

/**
* @brief Open a cv::namedWindow and display the img
Expand All @@ -53,7 +54,8 @@ class DebugVisualizer {

// Setters
/**
* @brief Value passed to cv::waitKey. Defines how long the image should be displayed. If 0 is specified, the window
* @brief Value passed to cv::waitKey. Defines how long the image should be displayed. If 0 is
* specified, the window
* will stay open until a key is pressed.
*
* @param milliseconds
Expand All @@ -65,7 +67,8 @@ class DebugVisualizer {
/**
* @brief Setter for the window mode to use.
*
* @param mode Can be any of CV_WINDOW_NORMAL (set by default), CV_WINDOW_AUTOSIZE, CV_WINDOW_OPENGL
* @param mode Can be any of CV_WINDOW_NORMAL (set by default), CV_WINDOW_AUTOSIZE,
* CV_WINDOW_OPENGL
*/
void SetWindowMode(int mode) {
m_window_mode = mode;
Expand Down Expand Up @@ -103,18 +106,20 @@ class DebugVisualizer {
* @param camera_intrinsics Camera parameters
* @param ego_pose Camera pose
*/
void DrawLandmarks(cv::Mat& img, const landmark_map_t& landmarks, const camera_params_t& camera_intrinsics,
void DrawLandmarks(cv::Mat& img,
const landmark_map_t& landmarks,
const camera_params_t& camera_intrinsics,
const pose_t& ego_pose);

private:
int m_wait_time; /**< Time to wait when displaying image */
int m_window_mode; /**< cvWindowProperty */
cv::Mat baseImg; /**< dummy image */
/**
* @brief Converts image to color image
*
* @param img
*/
int m_wait_time{1}; /**< Time to wait when displaying image */
int m_window_mode{CV_WINDOW_NORMAL}; /**< cvWindowProperty */
cv::Mat baseImg; /**< dummy image */
/**
* @brief Converts image to color image
*
* @param img
*/
void prepareImg(cv::Mat& img);
};

Expand Down
25 changes: 16 additions & 9 deletions include/stargazer/LandmarkCalibrator.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,8 @@
namespace stargazer {

/**
* @brief This is the class responsible for map generation. It computes a full bundle adjustment SLAM optimizing all
* @brief This is the class responsible for map generation. It computes a full bundle adjustment
* SLAM optimizing all
* observations of a full calibration sequenz in one optimization problem.
*
*/
Expand All @@ -42,14 +43,16 @@ class LandmarkCalibrator {
LandmarkCalibrator(std::string cfgfile);

/**
* @brief Adds all residual blocks to the problem. For every marker of every seen landmark at every pose a residual
* @brief Adds all residual blocks to the problem. For every marker of every seen landmark at
* every pose a residual
* block is added to the problem.
*
* @param observed_poses Initial guess of the cameras poses
* @param observed_landmarks Vector of all observed Image landmarks
*/
void AddReprojectionResidualBlocks(const std::vector<pose_t>& observed_poses,
const std::vector<std::vector<ImgLandmark>>& observed_landmarks);
void AddReprojectionResidualBlocks(
const std::vector<pose_t>& observed_poses,
const std::vector<std::vector<ImgLandmark>>& observed_landmarks);

/**
* @brief Main worker function. It calls the solver of the underlying ceres library.
Expand All @@ -62,21 +65,24 @@ class LandmarkCalibrator {
*/
void SetParametersConstant();
/**
* @brief Sets an individual landmarks pose constant. This is usefull, for fixing the maps coordinate system to one
* @brief Sets an individual landmarks pose constant. This is usefull, for fixing the maps
* coordinate system to one
* landmark.
*
* @param id Id of the landmark to hold constant.
*/
void SetLandmarkConstant(landmark_map_t::key_type id);
/**
* @brief Sets an individual camera pose constant. This is usefull, for fixing the maps coordinate system to one
* @brief Sets an individual camera pose constant. This is usefull, for fixing the maps
* coordinate system to one
* pose (normally the first).
*
* @param id
*/
void SetPoseConstant(size_t id);
/**
* @brief Removes all parameter and residual blocks from the problem. So that one can start from scratch.
* @brief Removes all parameter and residual blocks from the problem. So that one can start from
* scratch.
*
*/
void ClearProblem();
Expand Down Expand Up @@ -111,8 +117,9 @@ class LandmarkCalibrator {
private:
ceres::Problem problem; /**< Ceres problem */
camera_params_t camera_intrinsics_; /**< Camera parameters */
landmark_map_t landmarks_; /**< Map of landmarks. Points have to be defined in landmark coordinates!*/
std::vector<pose_t> camera_poses_; /**< Camera poses */
landmark_map_t
landmarks_; /**< Map of landmarks. Points have to be defined in landmark coordinates!*/
std::vector<pose_t> camera_poses_; /**< Camera poses */
};

} // namespace stargazer
20 changes: 12 additions & 8 deletions include/stargazer/StargazerTypes.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,8 @@ typedef std::array<double, (int)POINT::N_PARAMS> Point;
*/
typedef std::array<double, (int)INTRINSICS::N_PARAMS> camera_params_t;
/**
* @brief This object hold the parameters of a translation and orientation pose. See ::POSE for the indexing scheme.
* @brief This object hold the parameters of a translation and orientation pose. See ::POSE for
* the indexing scheme.
*
*/
typedef std::array<double, (int)POSE::N_PARAMS> pose_t;
Expand All @@ -64,12 +65,14 @@ typedef std::array<double, (int)POSE::N_PARAMS> pose_t;
* @brief Point generator function for a given ID.
*
* @param ID Landmark ID
* @return std::vector<Point> List of points in landmark coordinates. The first three are the three corner points.
* @return std::vector<Point> List of points in landmark coordinates. The first three are the three
* corner points.
*/
std::vector<Point> getLandmarkPoints(int ID); // Forward declaration

/**
* @brief This class resembles a map landmark. After construction with the id, the landmark holds its marker points in
* @brief This class resembles a map landmark. After construction with the id, the landmark holds
* its marker points in
* landmark coordinates.
*
*/
Expand Down Expand Up @@ -104,9 +107,10 @@ struct Landmark {
*/
Landmark(int ID) : id(ID), points(getLandmarkPoints(ID)){};

int id; /**< The landmarks id */
std::array<double, (int)POSE::N_PARAMS> pose = {{0., 0., 0., 0., 0., 0.}}; /**< The landmarks pose */
std::vector<Point> points; /**< Vector of landmark points. The first three are the corners */
int id; /**< The landmarks id */
std::array<double, (int)POSE::N_PARAMS> pose = {
{0., 0., 0., 0., 0., 0.}}; /**< The landmarks pose */
std::vector<Point> points; /**< Vector of landmark points. The first three are the corners */
static constexpr int kGridCount = 4; /**< Defines how many rows and columns the landmark has */
static constexpr double kGridDistance =
0.08; /**< Defines the distance between two landmark LEDs in meters. This is important for
Expand Down Expand Up @@ -154,10 +158,10 @@ inline std::vector<Point> getLandmarkPoints(int ID) {
ID -= col;
// Convert to binary
for (int x = 0; x < Landmark::kGridCount; x++) { // For every row
if (col % 2 != 0) { // Modulo 2 effectively converts the number to binary.
if (col % 2 != 0) { // Modulo 2 effectively converts the number to binary.
// If this returns 1, we have a point
// Point found
int id = y * Landmark::kGridCount + x;
// int id = y * Landmark::kGridCount + x;
Point pt = {x * Landmark::kGridDistance, y * Landmark::kGridDistance, 0};
points.push_back(pt);
}
Expand Down
30 changes: 17 additions & 13 deletions src/DebugVisualizer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,11 +21,6 @@

using namespace stargazer;

DebugVisualizer::DebugVisualizer() : m_window_mode(CV_WINDOW_NORMAL), m_wait_time(1) {
}
// TODO make this a singleton
// TODO make waitKey unique

void DebugVisualizer::prepareImg(cv::Mat& img) {
if (img.type() == CV_8UC1) {
// input image is grayscale
Expand All @@ -48,7 +43,8 @@ cv::Mat DebugVisualizer::ShowPoints(const cv::Mat& img, const std::vector<cv::Po
return temp;
}

cv::Mat DebugVisualizer::ShowClusters(const cv::Mat& img, const std::vector<std::vector<cv::Point>> points) {
cv::Mat DebugVisualizer::ShowClusters(const cv::Mat& img,
const std::vector<std::vector<cv::Point>> points) {
cv::Mat temp = img.clone();
prepareImg(temp);
for (auto& group : points) {
Expand Down Expand Up @@ -80,9 +76,10 @@ void DebugVisualizer::DrawLandmarks(cv::Mat& img, const std::vector<ImgLandmark>
for (auto& imgPoint : lm.voIDPoints) {
circle(img, imgPoint, 1, cv::Scalar(73, 119, 0), 2); // FZI Green
}
cv::Point median{(lm.voCorners[2].x + lm.voCorners[0].x) / 2, (lm.voCorners[2].y + lm.voCorners[0].y) / 2};
double radius =
sqrt(pow(lm.voCorners[2].x - lm.voCorners[0].x, 2) + pow(lm.voCorners[2].y - lm.voCorners[0].y, 2));
cv::Point median{(lm.voCorners[2].x + lm.voCorners[0].x) / 2,
(lm.voCorners[2].y + lm.voCorners[0].y) / 2};
double radius = sqrt(pow(lm.voCorners[2].x - lm.voCorners[0].x, 2) +
pow(lm.voCorners[2].y - lm.voCorners[0].y, 2));
circle(img, median, radius, cv::Scalar(163, 101, 0), 2); // FZI Blue

std::string text = "ID: ";
Expand All @@ -94,8 +91,10 @@ void DebugVisualizer::DrawLandmarks(cv::Mat& img, const std::vector<ImgLandmark>
}
}

void DebugVisualizer::DrawLandmarks(cv::Mat& img, const landmark_map_t& landmarks,
const camera_params_t& camera_intrinsics, const pose_t& ego_pose) {
void DebugVisualizer::DrawLandmarks(cv::Mat& img,
const landmark_map_t& landmarks,
const camera_params_t& camera_intrinsics,
const pose_t& ego_pose) {
cv::Point imgPoint;

for (auto& lm : landmarks) {
Expand All @@ -106,8 +105,13 @@ void DebugVisualizer::DrawLandmarks(cv::Mat& img, const landmark_map_t& landmark
double x = 0.0;
double y = 0.0;

transformWorldToImg(pt[(int)POINT::X], pt[(int)POINT::Y], pt[(int)POINT::Z], ego_pose.data(),
camera_intrinsics.data(), &x, &y);
transformWorldToImg(pt[(int)POINT::X],
pt[(int)POINT::Y],
pt[(int)POINT::Z],
ego_pose.data(),
camera_intrinsics.data(),
&x,
&y);
imgPoint.x = static_cast<int>(x);
imgPoint.y = static_cast<int>(y);

Expand Down
56 changes: 35 additions & 21 deletions src/LandmarkCalibrator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,14 @@ LandmarkCalibrator::LandmarkCalibrator(std::string cfgfile) {
};

void LandmarkCalibrator::AddReprojectionResidualBlocks(
const std::vector<pose_t>& observed_poses, const std::vector<std::vector<ImgLandmark>>& observed_landmarks) {
const std::vector<pose_t>& observed_poses,
const std::vector<std::vector<ImgLandmark>>& observed_landmarks) {

if (observed_landmarks.size() != observed_poses.size())
throw std::runtime_error("Got different amount of observations for landmarks and poses");

// Copy poses, as we keep and modify them. Landmark observations get only copied into costfunctor.
// Copy poses, as we keep and modify them. Landmark observations get only copied into
// costfunctor.
camera_poses_ = observed_poses;

for (size_t i = 0; i < observed_landmarks.size(); i++) {
Expand All @@ -44,8 +46,10 @@ void LandmarkCalibrator::AddReprojectionResidualBlocks(

Landmark& real_lm = landmarks_.at(observation.nID);

if (observation.voCorners.size() + observation.voIDPoints.size() != real_lm.points.size())
throw std::runtime_error("Observed Landmark has different ammount of points then real landmark!");
if (observation.voCorners.size() + observation.voIDPoints.size() !=
real_lm.points.size())
throw std::runtime_error(
"Observed Landmark has different ammount of points then real landmark!");

// Add residual block, for every one of the seen points.
for (size_t k = 0; k < real_lm.points.size(); k++) {
Expand All @@ -57,24 +61,33 @@ void LandmarkCalibrator::AddReprojectionResidualBlocks(

// Test reprojection error
double u, v;
transformLandMarkToImage<double>(real_lm.points[k][(int)POINT::X], real_lm.points[k][(int)POINT::Y],
real_lm.pose.data(), camera_poses_[i].data(),
camera_intrinsics_.data(), &u, &v);

ceres::CostFunction* cost_function = LandMarkToImageReprojectionFunctor::Create(
point_under_test->x, point_under_test->y, real_lm.points[k][(int)POINT::X],
real_lm.points[k][(int)POINT::Y]);
transformLandMarkToImage<double>(real_lm.points[k][(int)POINT::X],
real_lm.points[k][(int)POINT::Y],
real_lm.pose.data(),
camera_poses_[i].data(),
camera_intrinsics_.data(),
&u,
&v);

ceres::CostFunction* cost_function =
LandMarkToImageReprojectionFunctor::Create(point_under_test->x,
point_under_test->y,
real_lm.points[k][(int)POINT::X],
real_lm.points[k][(int)POINT::Y]);

problem.AddResidualBlock(cost_function,
new ceres::CauchyLoss(50), // NULL /* squared loss */, //
// Alternatively: new
// ceres::ScaledLoss(NULL, w_v_des,
// ceres::TAKE_OWNERSHIP),
real_lm.pose.data(), camera_poses_[i].data(), camera_intrinsics_.data());
// ceres::ScaledLoss(NULL, w_v_des,
// ceres::TAKE_OWNERSHIP),
real_lm.pose.data(),
camera_poses_[i].data(),
camera_intrinsics_.data());
}
}
problem.SetParameterization(camera_poses_[i].data(),
new ceres::SubsetParameterization((int)POSE::N_PARAMS, {{(int)POSE::Z}}));
problem.SetParameterization(
camera_poses_[i].data(),
new ceres::SubsetParameterization((int)POSE::N_PARAMS, {{(int)POSE::Z}}));
camera_poses_[i][(int)POSE::Z] = 0.0;
}
}
Expand Down Expand Up @@ -120,18 +133,19 @@ void LandmarkCalibrator::SetParametersConstant() {

// Set Landmark rotation parameters constant
std::vector<int> constant_landmark_params = {(int)POSE::Rx, (int)POSE::Ry, (int)POSE::Rz};
ceres::SubsetParameterization* constant_landmark_params_mask =
new ceres::SubsetParameterization((int)POSE::N_PARAMS, constant_landmark_params);
// ceres::SubsetParameterization* constant_landmark_params_mask =
// new ceres::SubsetParameterization((int)POSE::N_PARAMS, constant_landmark_params);
for (auto& lm : landmarks_) {
if (problem.HasParameterBlock(lm.second.pose.data()))
// problem.SetParameterization(lm.second.pose.data(), constant_landmark_params_mask);
// problem.SetParameterization(lm.second.pose.data(),
// constant_landmark_params_mask);
problem.SetParameterBlockConstant(lm.second.pose.data());
}

// Set some pose parameters constant
std::vector<int> constant_vehicle_params = {(int)POSE::Z, (int)POSE::Rx, (int)POSE::Ry};
ceres::SubsetParameterization* constant_vehicle_params_mask =
new ceres::SubsetParameterization((int)POSE::N_PARAMS, constant_vehicle_params);
// ceres::SubsetParameterization* constant_vehicle_params_mask =
// new ceres::SubsetParameterization((int)POSE::N_PARAMS, constant_vehicle_params);
for (auto& pose : camera_poses_) {
if (problem.HasParameterBlock(pose.data()))
// problem.SetParameterization(pose.data(), constant_vehicle_params_mask);
Expand Down

0 comments on commit 85a911d

Please sign in to comment.