Skip to content

Commit

Permalink
working stereo
Browse files Browse the repository at this point in the history
  • Loading branch information
@raphaelchang
raphaelchang committed Sep 19, 2019
1 parent 9fbf770 commit 3a62f35
Show file tree
Hide file tree
Showing 36 changed files with 982 additions and 157 deletions.
28 changes: 27 additions & 1 deletion CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ include_directories(
add_library(omni_slam_eval_lib
src/module/tracking_module.cc
src/module/matching_module.cc
src/module/stereo_module.cc
src/module/reconstruction_module.cc
src/module/odometry_module.cc
src/data/frame.cc
Expand All @@ -70,6 +71,8 @@ add_library(omni_slam_eval_lib
src/odometry/pnp.cc
src/odometry/five_point.cc
src/optimization/bundle_adjuster.cc
src/stereo/stereo_matcher.cc
src/stereo/lk_stereo_matcher.cc
src/util/hdf_file.cc
)

Expand Down Expand Up @@ -99,6 +102,13 @@ add_executable(omni_slam_odometry_eval_node
src/omni_slam_odometry_eval_node.cc
)

add_executable(omni_slam_stereo_eval_node
src/ros/stereo_eval.cc
src/ros/tracking_eval.cc
src/ros/eval_base.cc
src/omni_slam_stereo_eval_node.cc
)

target_link_libraries(omni_slam_tracking_eval_node PUBLIC omni_slam_eval_lib
${catkin_LIBRARIES}
${OpenCV_LIBS}
Expand Down Expand Up @@ -135,11 +145,21 @@ target_link_libraries(omni_slam_odometry_eval_node PUBLIC omni_slam_eval_lib
${CERES_LIBRARIES}
)

target_link_libraries(omni_slam_stereo_eval_node PUBLIC omni_slam_eval_lib
${catkin_LIBRARIES}
${OpenCV_LIBS}
${HDF5_CXX_LIBRARIES}
${OpenMP_CXX_FLAGS}
${PCL_LIBRARIES}
${CERES_LIBRARIES}
)

target_compile_options(omni_slam_tracking_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
target_compile_options(omni_slam_matching_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
target_compile_options(omni_slam_reconstruction_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
target_compile_options(omni_slam_odometry_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
set(CMAKE_CXX_FLAGS ${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS})
target_compile_options(omni_slam_stereo_eval_node PUBLIC ${OpenMP_CXX_FLAGS})
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS} -ggdb")

install(TARGETS omni_slam_tracking_eval_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
Expand All @@ -164,3 +184,9 @@ install(TARGETS omni_slam_odometry_eval_node
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)

install(TARGETS omni_slam_stereo_eval_node
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
33 changes: 33 additions & 0 deletions launch/stereo_eval.launch
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
<launch>
<arg name="bag_file" default="" />
<arg name="results_file" default="$(arg bag_file).stereo.hdf5" />
<arg name="camera_model" default="double_sphere" />
<arg name="camera_params" default="{fx: 295.936, fy: 295.936, cx: 512, cy: 512, chi: 0.3, alpha: 0.6666667}" />
<arg name="rate" default="1" />
<node pkg="omni_slam_eval" type="omni_slam_stereo_eval_node" name="omni_slam_stereo_eval_node" required="true" output="screen">
<param name="bag_file" value="$(arg bag_file)" />
<param name="results_file" value="$(arg results_file)" />
<param name="image_topic" value="/unity_ros/Sphere/FisheyeCamera/image_raw" />
<param name="stereo_image_topic" value="/unity_ros/Sphere/FisheyeCamera2/image_raw" />
<param name="depth_image_topic" value="/unity_ros/Sphere/FisheyeDepthCamera/image_raw" />
<param name="pose_topic" value="/unity_ros/Sphere/TrueState/pose" />
<param name="stereo_matched_topic" value="/omni_slam/stereo_matched" />
<param name="rate" value="$(arg rate)" />
<rosparam subst_value="true">
camera_model: '$(arg camera_model)'
camera_parameters: $(arg camera_params)
detector_type: 'GFTT'
detector_parameters: {maxCorners: 1000, qualityLevel: 0.001, minDistance: 5, blockSize: 5}
stereo_matcher_window_size: 256
stereo_matcher_num_scales: 5
stereo_matcher_error_threshold: 20
stereo_matcher_epipolar_threshold: 0.008
stereo_tf_t: [2.0, 0.0, 0.0]
stereo_tf_r: [0.0, 0.0, 0.0, 1.0]
</rosparam>
</node>
</launch>




5 changes: 3 additions & 2 deletions src/data/feature.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,14 +28,15 @@ class Feature
bool HasWorldPoint() const;
bool HasEstimatedWorldPoint() const;

bool worldPointCached_{false};
bool worldPointEstimateCached_{false};
private:
Frame &frame_;
cv::KeyPoint kpt_;
cv::Mat descriptor_;
Vector3d worldPoint_;
Vector3d worldPointEstimate_;

bool worldPointCached_{false};
bool worldPointEstimateCached_{false};
};

}
Expand Down
115 changes: 114 additions & 1 deletion src/data/frame.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,17 +8,33 @@ namespace data

int Frame::lastFrameId_ = 0;

Frame::Frame(cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4> &pose, double time, camera::CameraModel<> &camera_model)
Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
: id_(lastFrameId_++),
image_(image.clone()),
stereoImage_(stereo_image.clone()),
depthImage_(depth_image.clone()),
pose_(pose),
invPose_(util::TFUtil::InversePoseMatrix(pose)),
stereoPose_(stereo_pose),
timeSec_(time),
cameraModel_(camera_model)
{
hasPose_ = true;
hasDepth_ = true;
hasStereo_ = true;
}

Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
: id_(lastFrameId_++),
image_(image.clone()),
stereoImage_(stereo_image.clone()),
stereoPose_(stereo_pose),
timeSec_(time),
cameraModel_(camera_model)
{
hasPose_ = false;
hasDepth_ = false;
hasStereo_ = true;
}

Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
Expand All @@ -30,6 +46,21 @@ Frame::Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraMo
{
hasPose_ = false;
hasDepth_ = true;
hasStereo_ = false;
}

Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
: id_(lastFrameId_++),
image_(image.clone()),
stereoImage_(stereo_image.clone()),
depthImage_(depth_image.clone()),
stereoPose_(stereo_pose),
timeSec_(time),
cameraModel_(camera_model)
{
hasPose_ = false;
hasDepth_ = true;
hasStereo_ = true;
}

Frame::Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, double time, camera::CameraModel<> &camera_model)
Expand All @@ -42,6 +73,36 @@ Frame::Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, double time, camera::C
{
hasPose_ = true;
hasDepth_ = false;
hasStereo_ = false;
}

Frame::Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model)
: id_(lastFrameId_++),
image_(image.clone()),
stereoImage_(stereo_image.clone()),
pose_(pose),
invPose_(util::TFUtil::InversePoseMatrix(pose)),
stereoPose_(stereo_pose),
timeSec_(time),
cameraModel_(camera_model)
{
hasPose_ = true;
hasDepth_ = false;
hasStereo_ = true;
}

Frame::Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model)
: id_(lastFrameId_++),
image_(image.clone()),
depthImage_(depth_image.clone()),
pose_(pose),
invPose_(util::TFUtil::InversePoseMatrix(pose)),
timeSec_(time),
cameraModel_(camera_model)
{
hasPose_ = true;
hasDepth_ = true;
hasStereo_ = false;
}

Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
Expand All @@ -51,20 +112,24 @@ Frame::Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model)
{
hasPose_ = false;
hasDepth_ = false;
hasStereo_ = false;
}

Frame::Frame(const Frame &other)
: image_(other.image_.clone()),
depthImage_(other.depthImage_.clone()),
stereoImage_(other.stereoImage_.clone()),
imageComp_(other.imageComp_),
depthImageComp_(other.depthImageComp_),
stereoImageComp_(other.stereoImageComp_),
cameraModel_(other.cameraModel_),
id_(other.id_),
pose_(other.pose_),
invPose_(other.invPose_),
timeSec_(other.timeSec_),
hasPose_(other.hasPose_),
hasDepth_(other.hasDepth_),
hasStereo_(other.hasStereo_),
isCompressed_(other.isCompressed_)
{
}
Expand Down Expand Up @@ -97,11 +162,25 @@ const cv::Mat& Frame::GetDepthImage()
return depthImage_;
}

const cv::Mat& Frame::GetStereoImage()
{
if (isCompressed_)
{
DecompressImages();
}
return stereoImage_;
}

const int Frame::GetID() const
{
return id_;
}

const Matrix<double, 3, 4>& Frame::GetStereoPose() const
{
return stereoPose_;
}

const camera::CameraModel<>& Frame::GetCameraModel() const
{
return cameraModel_;
Expand Down Expand Up @@ -138,6 +217,25 @@ void Frame::SetDepthImage(cv::Mat &depth_image)
hasDepth_ = true;
}

void Frame::SetStereoImage(cv::Mat &stereo_image)
{
if (isCompressed_)
{
std::vector<int> param = {cv::IMWRITE_PNG_COMPRESSION, 5};
cv::imencode(".png", stereo_image, stereoImageComp_, param);
}
else
{
stereoImageComp_ = stereo_image.clone();
}
hasStereo_ = true;
}

void Frame::SetStereoPose(Matrix<double, 3, 4> &pose)
{
stereoPose_ = pose;
}

void Frame::SetEstimatedPose(const Matrix<double, 3, 4> &pose, const std::vector<int> &landmark_ids)
{
poseEstimate_ = pose;
Expand Down Expand Up @@ -178,6 +276,11 @@ bool Frame::HasDepthImage() const
return hasDepth_;
}

bool Frame::HasStereoImage() const
{
return hasStereo_;
}

bool Frame::HasEstimatedPose() const
{
return hasPoseEstimate_;
Expand All @@ -200,8 +303,13 @@ void Frame::CompressImages()
{
cv::imencode(".png", depthImage_, depthImageComp_, param);
}
if (hasStereo_)
{
cv::imencode(".png", stereoImage_, stereoImageComp_, param);
}
image_.release();
depthImage_.release();
stereoImage_.release();
isCompressed_ = true;
}

Expand All @@ -216,8 +324,13 @@ void Frame::DecompressImages()
{
depthImage_ = cv::imdecode(cv::Mat(1, depthImageComp_.size(), CV_8UC1, depthImageComp_.data()), CV_LOAD_IMAGE_UNCHANGED);
}
if (hasStereo_)
{
stereoImage_ = cv::imdecode(cv::Mat(1, stereoImageComp_.size(), CV_8UC1, stereoImageComp_.data()), CV_LOAD_IMAGE_UNCHANGED);
}
imageComp_.clear();
depthImageComp_.clear();
stereoImageComp_.clear();
isCompressed_ = false;
}

Expand Down
15 changes: 14 additions & 1 deletion src/data/frame.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,16 +16,22 @@ namespace data
class Frame
{
public:
Frame(cv::Mat &image, cv::Mat &depth_image, Matrix<double, 3, 4> &pose, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, cv::Mat &stereo_image, cv::Mat &depth_image, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, cv::Mat &stereo_image, Matrix<double, 3, 4> &pose, Matrix<double, 3, 4> &stereo_pose, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, Matrix<double, 3, 4> &pose, cv::Mat &depth_image, double time, camera::CameraModel<> &camera_model);
Frame(cv::Mat &image, double time, camera::CameraModel<> &camera_model);
Frame(const Frame &other);

const Matrix<double, 3, 4>& GetPose() const;
const Matrix<double, 3, 4>& GetInversePose() const;
const cv::Mat& GetImage();
const cv::Mat& GetDepthImage();
const cv::Mat& GetStereoImage();
const Matrix<double, 3, 4>& GetStereoPose() const;
const camera::CameraModel<>& GetCameraModel() const;
const Matrix<double, 3, 4>& GetEstimatedPose() const;
const Matrix<double, 3, 4>& GetEstimatedInversePose() const;
Expand All @@ -34,11 +40,14 @@ class Frame

bool HasPose() const;
bool HasDepthImage() const;
bool HasStereoImage() const;
bool HasEstimatedPose() const;
bool IsEstimatedByLandmark(const int landmark_id) const;

void SetPose(Matrix<double, 3, 4> &pose);
void SetDepthImage(cv::Mat &depth_image);
void SetStereoImage(cv::Mat &stereo_image);
void SetStereoPose(Matrix<double, 3, 4> &pose);
void SetEstimatedPose(const Matrix<double, 3, 4> &pose, const std::vector<int> &landmark_ids);
void SetEstimatedPose(const Matrix<double, 3, 4> &pose);
void SetEstimatedInversePose(const Matrix<double, 3, 4> &pose, const std::vector<int> &landmark_ids);
Expand All @@ -52,10 +61,13 @@ class Frame
const int id_;
std::vector<unsigned char> imageComp_;
std::vector<unsigned char> depthImageComp_;
std::vector<unsigned char> stereoImageComp_;
cv::Mat image_;
cv::Mat depthImage_;
cv::Mat stereoImage_;
Matrix<double, 3, 4> pose_;
Matrix<double, 3, 4> invPose_;
Matrix<double, 3, 4> stereoPose_;
Matrix<double, 3, 4> poseEstimate_;
Matrix<double, 3, 4> invPoseEstimate_;
double timeSec_;
Expand All @@ -65,6 +77,7 @@ class Frame

bool hasPose_;
bool hasDepth_;
bool hasStereo_;
bool hasPoseEstimate_{false};

bool isCompressed_{false};
Expand Down
Loading

0 comments on commit 3a62f35

Please sign in to comment.