Compare commits
29 Commits
photometry
...
photometry
Author | SHA1 | Date | |
---|---|---|---|
2dac361ba2 | |||
0712a98c7f | |||
6b8dce9876 | |||
49374a4323 | |||
3e480560e8 | |||
b3df525060 | |||
a8d4580812 | |||
a0577dfb9d | |||
8cfbe06945 | |||
cab56d9494 | |||
5e9149eacc | |||
e4dbe2f060 | |||
6b208dbc44 | |||
2ee7c248c1 | |||
44de215518 | |||
ad2f464716 | |||
53b26f7613 | |||
ee40dff740 | |||
acbcf79417 | |||
cf40ce8afb | |||
750d8ecfb1 | |||
e3ac604dbf | |||
e8489dbd06 | |||
e2e936ff01 | |||
de07296d31 | |||
6ba26d782d | |||
821d9d6f71 | |||
1ffc4fb37f | |||
5958adb57c |
@ -24,6 +24,7 @@ find_package(catkin REQUIRED COMPONENTS
|
||||
pcl_conversions
|
||||
pcl_ros
|
||||
std_srvs
|
||||
visualization_msgs
|
||||
)
|
||||
|
||||
## System dependencies are found with CMake's conventions
|
||||
|
@ -15,6 +15,10 @@
|
||||
#include <Eigen/Dense>
|
||||
#include <Eigen/Geometry>
|
||||
#include <Eigen/StdVector>
|
||||
#include <math.h>
|
||||
#include <visualization_msgs/Marker.h>
|
||||
#include <visualization_msgs/MarkerArray.h>
|
||||
#include <geometry_msgs/Point.h>
|
||||
|
||||
#include "image_handler.h"
|
||||
|
||||
@ -22,6 +26,7 @@
|
||||
#include "imu_state.h"
|
||||
#include "cam_state.h"
|
||||
|
||||
|
||||
namespace msckf_vio {
|
||||
|
||||
/*
|
||||
@ -65,6 +70,11 @@ struct Feature {
|
||||
position(Eigen::Vector3d::Zero()),
|
||||
is_initialized(false), is_anchored(false) {}
|
||||
|
||||
|
||||
void Rhocost(const Eigen::Isometry3d& T_c0_ci,
|
||||
const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
|
||||
double& e) const;
|
||||
|
||||
/*
|
||||
* @brief cost Compute the cost of the camera observations
|
||||
* @param T_c0_c1 A rigid body transformation takes
|
||||
@ -77,6 +87,13 @@ struct Feature {
|
||||
const Eigen::Vector3d& x, const Eigen::Vector2d& z,
|
||||
double& e) const;
|
||||
|
||||
bool initializeRho(const CamStateServer& cam_states);
|
||||
|
||||
inline void RhoJacobian(const Eigen::Isometry3d& T_c0_ci,
|
||||
const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
|
||||
Eigen::Matrix<double, 2, 1>& J, Eigen::Vector2d& r,
|
||||
double& w) const;
|
||||
|
||||
/*
|
||||
* @brief jacobian Compute the Jacobian of the camera observation
|
||||
* @param T_c0_c1 A rigid body transformation takes
|
||||
@ -92,6 +109,10 @@ struct Feature {
|
||||
Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
|
||||
double& w) const;
|
||||
|
||||
inline double generateInitialDepth(
|
||||
const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
|
||||
const Eigen::Vector2d& z2) const;
|
||||
|
||||
/*
|
||||
* @brief generateInitialGuess Compute the initial guess of
|
||||
* the feature's 3d position using only two views.
|
||||
@ -126,7 +147,7 @@ struct Feature {
|
||||
*/
|
||||
|
||||
bool initializeAnchor(
|
||||
const CameraCalibration& cam);
|
||||
const CameraCalibration& cam, int N);
|
||||
|
||||
|
||||
/*
|
||||
@ -143,6 +164,14 @@ struct Feature {
|
||||
inline bool initializePosition(
|
||||
const CamStateServer& cam_states);
|
||||
|
||||
cv::Point2f pixelDistanceAt(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
const CameraCalibration& cam,
|
||||
Eigen::Vector3d& in_p) const;
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* @brief project PositionToCamera Takes a 3d position in a world frame
|
||||
* and projects it into the passed camera frame using pinhole projection
|
||||
@ -155,6 +184,11 @@ struct Feature {
|
||||
const CameraCalibration& cam,
|
||||
Eigen::Vector3d& in_p) const;
|
||||
|
||||
double Kernel(
|
||||
const cv::Point2f pose,
|
||||
const cv::Mat frame,
|
||||
std::string type) const;
|
||||
|
||||
/*
|
||||
* @brief IrradianceAnchorPatch_Camera returns irradiance values
|
||||
* of the Anchor Patch position in a camera frame
|
||||
@ -165,19 +199,26 @@ struct Feature {
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0,
|
||||
std::vector<float>& anchorPatch_estimate) const;
|
||||
std::vector<double>& anchorPatch_estimate,
|
||||
IlluminationParameter& estimatedIllumination) const;
|
||||
|
||||
bool FrameIrradiance(
|
||||
bool MarkerGeneration(
|
||||
ros::Publisher& marker_pub,
|
||||
const CamStateServer& cam_states) const;
|
||||
|
||||
bool VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0,
|
||||
std::vector<float>& anchorPatch_measurement) const;
|
||||
|
||||
const Eigen::VectorXd& photo_r,
|
||||
std::stringstream& ss,
|
||||
cv::Point2f gradientVector,
|
||||
cv::Point2f residualVector) const;
|
||||
/*
|
||||
* @brief projectPixelToPosition uses the calcualted pixels
|
||||
* @brief AnchorPixelToPosition uses the calcualted pixels
|
||||
* of the anchor patch to generate 3D positions of all of em
|
||||
*/
|
||||
inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
|
||||
inline Eigen::Vector3d AnchorPixelToPosition(cv::Point2f in_p,
|
||||
const CameraCalibration& cam);
|
||||
|
||||
/*
|
||||
@ -203,8 +244,10 @@ inline Eigen::Vector3d projectPixelToPosition(cv::Point2f in_p,
|
||||
|
||||
// NxN Patch of Anchor Image
|
||||
std::vector<double> anchorPatch;
|
||||
std::vector<cv::Point2f> anchorPatch_ideal;
|
||||
std::vector<cv::Point2f> anchorPatch_real;
|
||||
|
||||
// Position of NxN Patch in 3D space
|
||||
// Position of NxN Patch in 3D space in anchor camera frame
|
||||
std::vector<Eigen::Vector3d> anchorPatch_3d;
|
||||
|
||||
// Anchor Isometry
|
||||
@ -236,10 +279,31 @@ typedef std::map<FeatureIDType, Feature, std::less<int>,
|
||||
Eigen::aligned_allocator<
|
||||
std::pair<const FeatureIDType, Feature> > > MapServer;
|
||||
|
||||
void Feature::Rhocost(const Eigen::Isometry3d& T_c0_ci,
|
||||
const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
|
||||
double& e) const
|
||||
{
|
||||
// Compute hi1, hi2, and hi3 as Equation (37).
|
||||
const double& rho = x;
|
||||
|
||||
Eigen::Vector3d h = T_c0_ci.linear()*
|
||||
Eigen::Vector3d(z1(0), z1(1), 1.0) + rho*T_c0_ci.translation();
|
||||
double& h1 = h(0);
|
||||
double& h2 = h(1);
|
||||
double& h3 = h(2);
|
||||
|
||||
// Predict the feature observation in ci frame.
|
||||
Eigen::Vector2d z_hat(h1/h3, h2/h3);
|
||||
|
||||
// Compute the residual.
|
||||
e = (z_hat-z2).squaredNorm();
|
||||
return;
|
||||
}
|
||||
|
||||
void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
|
||||
const Eigen::Vector3d& x, const Eigen::Vector2d& z,
|
||||
double& e) const {
|
||||
double& e) const
|
||||
{
|
||||
// Compute hi1, hi2, and hi3 as Equation (37).
|
||||
const double& alpha = x(0);
|
||||
const double& beta = x(1);
|
||||
@ -247,9 +311,9 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
|
||||
|
||||
Eigen::Vector3d h = T_c0_ci.linear()*
|
||||
Eigen::Vector3d(alpha, beta, 1.0) + rho*T_c0_ci.translation();
|
||||
double& h1 = h(0);
|
||||
double& h2 = h(1);
|
||||
double& h3 = h(2);
|
||||
double h1 = h(0);
|
||||
double h2 = h(1);
|
||||
double h3 = h(2);
|
||||
|
||||
// Predict the feature observation in ci frame.
|
||||
Eigen::Vector2d z_hat(h1/h3, h2/h3);
|
||||
@ -259,10 +323,47 @@ void Feature::cost(const Eigen::Isometry3d& T_c0_ci,
|
||||
return;
|
||||
}
|
||||
|
||||
void Feature::RhoJacobian(const Eigen::Isometry3d& T_c0_ci,
|
||||
const double x, const Eigen::Vector2d& z1, const Eigen::Vector2d& z2,
|
||||
Eigen::Matrix<double, 2, 1>& J, Eigen::Vector2d& r,
|
||||
double& w) const
|
||||
{
|
||||
|
||||
const double& rho = x;
|
||||
|
||||
Eigen::Vector3d h = T_c0_ci.linear()*
|
||||
Eigen::Vector3d(z1(0), z2(1), 1.0) + rho*T_c0_ci.translation();
|
||||
double& h1 = h(0);
|
||||
double& h2 = h(1);
|
||||
double& h3 = h(2);
|
||||
|
||||
// Compute the Jacobian.
|
||||
Eigen::Matrix3d W;
|
||||
W.leftCols<2>() = T_c0_ci.linear().leftCols<2>();
|
||||
W.rightCols<1>() = T_c0_ci.translation();
|
||||
|
||||
J(0,0) = -h1/(h3*h3);
|
||||
J(1,0) = -h2/(h3*h3);
|
||||
|
||||
// Compute the residual.
|
||||
Eigen::Vector2d z_hat(h1/h3, h2/h3);
|
||||
r = z_hat - z2;
|
||||
|
||||
// Compute the weight based on the residual.
|
||||
double e = r.norm();
|
||||
if (e <= optimization_config.huber_epsilon)
|
||||
w = 1.0;
|
||||
else
|
||||
w = optimization_config.huber_epsilon / (2*e);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
|
||||
const Eigen::Vector3d& x, const Eigen::Vector2d& z,
|
||||
Eigen::Matrix<double, 2, 3>& J, Eigen::Vector2d& r,
|
||||
double& w) const {
|
||||
double& w) const
|
||||
{
|
||||
|
||||
// Compute hi1, hi2, and hi3 as Equation (37).
|
||||
const double& alpha = x(0);
|
||||
@ -297,9 +398,10 @@ void Feature::jacobian(const Eigen::Isometry3d& T_c0_ci,
|
||||
return;
|
||||
}
|
||||
|
||||
void Feature::generateInitialGuess(
|
||||
double Feature::generateInitialDepth(
|
||||
const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
|
||||
const Eigen::Vector2d& z2, Eigen::Vector3d& p) const {
|
||||
const Eigen::Vector2d& z2) const
|
||||
{
|
||||
// Construct a least square problem to solve the depth.
|
||||
Eigen::Vector3d m = T_c1_c2.linear() * Eigen::Vector3d(z1(0), z1(1), 1.0);
|
||||
|
||||
@ -313,14 +415,23 @@ void Feature::generateInitialGuess(
|
||||
|
||||
// Solve for the depth.
|
||||
double depth = (A.transpose() * A).inverse() * A.transpose() * b;
|
||||
return depth;
|
||||
}
|
||||
|
||||
|
||||
void Feature::generateInitialGuess(
|
||||
const Eigen::Isometry3d& T_c1_c2, const Eigen::Vector2d& z1,
|
||||
const Eigen::Vector2d& z2, Eigen::Vector3d& p) const
|
||||
{
|
||||
double depth = generateInitialDepth(T_c1_c2, z1, z2);
|
||||
p(0) = z1(0) * depth;
|
||||
p(1) = z1(1) * depth;
|
||||
p(2) = depth;
|
||||
return;
|
||||
}
|
||||
|
||||
bool Feature::checkMotion(
|
||||
const CamStateServer& cam_states) const {
|
||||
bool Feature::checkMotion(const CamStateServer& cam_states) const
|
||||
{
|
||||
|
||||
const StateIDType& first_cam_id = observations.begin()->first;
|
||||
const StateIDType& last_cam_id = (--observations.end())->first;
|
||||
@ -362,11 +473,32 @@ bool Feature::checkMotion(
|
||||
else return false;
|
||||
}
|
||||
|
||||
double Feature::Kernel(
|
||||
const cv::Point2f pose,
|
||||
const cv::Mat frame,
|
||||
std::string type) const
|
||||
{
|
||||
Eigen::Matrix<double, 3, 3> kernel = Eigen::Matrix<double, 3, 3>::Zero();
|
||||
if(type == "Sobel_x")
|
||||
kernel << -1., 0., 1.,-2., 0., 2. , -1., 0., 1.;
|
||||
else if(type == "Sobel_y")
|
||||
kernel << -1., -2., -1., 0., 0., 0., 1., 2., 1.;
|
||||
|
||||
double delta = 0;
|
||||
int offs = (int)(kernel.rows()-1)/2;
|
||||
|
||||
for(int i = 0; i < kernel.rows(); i++)
|
||||
for(int j = 0; j < kernel.cols(); j++)
|
||||
delta += ((float)frame.at<uint8_t>(pose.y+j-offs , pose.x+i-offs))/255 * (float)kernel(j,i);
|
||||
|
||||
return delta;
|
||||
}
|
||||
bool Feature::estimate_FrameIrradiance(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0,
|
||||
std::vector<float>& anchorPatch_estimate) const
|
||||
std::vector<double>& anchorPatch_estimate,
|
||||
IlluminationParameter& estimated_illumination) const
|
||||
{
|
||||
// get irradiance of patch in anchor frame
|
||||
// subtract estimated b and divide by a of anchor frame
|
||||
@ -385,66 +517,371 @@ bool Feature::estimate_FrameIrradiance(
|
||||
double a_l = frameExposureTime_ms;
|
||||
double b_l = 0;
|
||||
|
||||
estimated_illumination.frame_gain = a_l;
|
||||
estimated_illumination.frame_bias = b_l;
|
||||
estimated_illumination.feature_gain = 1;
|
||||
estimated_illumination.feature_bias = 0;
|
||||
//printf("frames: %lld, %lld\n", anchor->first, cam_state_id);
|
||||
//printf("exposure: %f, %f\n", a_A, a_l);
|
||||
|
||||
for (double anchorPixel : anchorPatch)
|
||||
{
|
||||
float irradiance = ((anchorPixel - b_A) / a_A ) * a_l - b_l;
|
||||
float irradiance = (anchorPixel - b_A) / a_A ;
|
||||
anchorPatch_estimate.push_back(irradiance);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
bool Feature::FrameIrradiance(
|
||||
// generates markers for every camera position/observation
|
||||
// and estimated feature/path position
|
||||
bool Feature::MarkerGeneration(
|
||||
ros::Publisher& marker_pub,
|
||||
const CamStateServer& cam_states) const
|
||||
{
|
||||
visualization_msgs::MarkerArray ma;
|
||||
|
||||
// add all camera states used for estimation
|
||||
int count = 0;
|
||||
|
||||
for(auto observation : observations)
|
||||
{
|
||||
visualization_msgs::Marker marker;
|
||||
marker.header.frame_id = "world";
|
||||
marker.header.stamp = ros::Time::now();
|
||||
|
||||
marker.ns = "cameras";
|
||||
marker.id = count++;
|
||||
marker.type = visualization_msgs::Marker::ARROW;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
marker.pose.position.x = cam_states.find(observation.first)->second.position(0);
|
||||
marker.pose.position.y = cam_states.find(observation.first)->second.position(1);
|
||||
marker.pose.position.z = cam_states.find(observation.first)->second.position(2);
|
||||
|
||||
// rotate form x to z axis
|
||||
Eigen::Vector4d q = quaternionMultiplication(Eigen::Vector4d(0, -0.707, 0, 0.707), cam_states.find(observation.first)->second.orientation);
|
||||
|
||||
marker.pose.orientation.x = q(0);
|
||||
marker.pose.orientation.y = q(1);
|
||||
marker.pose.orientation.z = q(2);
|
||||
marker.pose.orientation.w = q(3);
|
||||
|
||||
marker.scale.x = 0.15;
|
||||
marker.scale.y = 0.05;
|
||||
marker.scale.z = 0.05;
|
||||
|
||||
if(count == 1)
|
||||
{
|
||||
marker.color.r = 0.0f;
|
||||
marker.color.g = 0.0f;
|
||||
marker.color.b = 1.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
marker.color.r = 0.0f;
|
||||
marker.color.g = 1.0f;
|
||||
marker.color.b = 0.0f;
|
||||
}
|
||||
marker.color.a = 1.0;
|
||||
|
||||
marker.lifetime = ros::Duration(0);
|
||||
|
||||
ma.markers.push_back(marker);
|
||||
}
|
||||
|
||||
// 'delete' any existing cameras (make invisible)
|
||||
for(int i = count; i < 20; i++)
|
||||
{
|
||||
visualization_msgs::Marker marker;
|
||||
|
||||
marker.header.frame_id = "world";
|
||||
marker.header.stamp = ros::Time::now();
|
||||
|
||||
marker.ns = "cameras";
|
||||
marker.id = i;
|
||||
marker.type = visualization_msgs::Marker::ARROW;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
marker.pose.orientation.w = 1;
|
||||
|
||||
marker.color.a = 0.0;
|
||||
|
||||
marker.lifetime = ros::Duration(1);
|
||||
|
||||
ma.markers.push_back(marker);
|
||||
}
|
||||
|
||||
//generate feature patch points position
|
||||
visualization_msgs::Marker marker;
|
||||
|
||||
marker.header.frame_id = "world";
|
||||
marker.header.stamp = ros::Time::now();
|
||||
|
||||
marker.ns = "patch";
|
||||
marker.id = 0;
|
||||
marker.type = visualization_msgs::Marker::POINTS;
|
||||
marker.action = visualization_msgs::Marker::ADD;
|
||||
|
||||
marker.pose.orientation.w = 1;
|
||||
|
||||
marker.scale.x = 0.02;
|
||||
marker.scale.y = 0.02;
|
||||
|
||||
marker.color.r = 1.0f;
|
||||
marker.color.g = 0.0f;
|
||||
marker.color.b = 0.0f;
|
||||
marker.color.a = 1.0;
|
||||
|
||||
for(auto point : anchorPatch_3d)
|
||||
{
|
||||
geometry_msgs::Point p;
|
||||
p.x = point(0);
|
||||
p.y = point(1);
|
||||
p.z = point(2);
|
||||
marker.points.push_back(p);
|
||||
}
|
||||
ma.markers.push_back(marker);
|
||||
|
||||
marker_pub.publish(ma);
|
||||
}
|
||||
|
||||
bool Feature::VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam0,
|
||||
std::vector<float>& anchorPatch_measurement) const
|
||||
const Eigen::VectorXd& photo_r,
|
||||
std::stringstream& ss,
|
||||
cv::Point2f gradientVector,
|
||||
cv::Point2f residualVector) const
|
||||
{
|
||||
|
||||
// visu - feature
|
||||
/*cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
cv::Mat dottedFrame(current_image.size(), CV_8UC3);
|
||||
cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
|
||||
*/
|
||||
double rescale = 1;
|
||||
|
||||
// project every point in anchorPatch_3d.
|
||||
//visu - anchor
|
||||
auto anchor = observations.begin();
|
||||
cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
|
||||
cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
|
||||
cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
|
||||
|
||||
// visualize the true anchor points (the surrounding of the original measurements)
|
||||
for(auto point : anchorPatch_real)
|
||||
{
|
||||
// visu - feature
|
||||
cv::Point xs(point.x, point.y);
|
||||
cv::Point ys(point.x, point.y);
|
||||
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
|
||||
}
|
||||
cam0.featureVisu = dottedFrame.clone();
|
||||
|
||||
// visu - feature
|
||||
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
|
||||
|
||||
// set position in frame
|
||||
// save irradiance of projection
|
||||
std::vector<double> projectionPatch;
|
||||
for(auto point : anchorPatch_3d)
|
||||
{
|
||||
|
||||
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam0, point);
|
||||
|
||||
projectionPatch.push_back(PixelIrradiance(p_in_c0, current_image));
|
||||
// visu - feature
|
||||
/*cv::Point xs(p_in_c0.x, p_in_c0.y);
|
||||
cv::Point xs(p_in_c0.x, p_in_c0.y);
|
||||
cv::Point ys(p_in_c0.x, p_in_c0.y);
|
||||
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
|
||||
*/
|
||||
float irradiance = PixelIrradiance(p_in_c0, cam0.moving_window.find(cam_state_id)->second.image);
|
||||
anchorPatch_measurement.push_back(irradiance);
|
||||
|
||||
// testing
|
||||
//if(cam_state_id == observations.begin()->first)
|
||||
//if(count++ == 4)
|
||||
//printf("dist:\n \tpos: %f, %f\n\ttrue pos: %f, %f\n\n", p_in_c0.x, p_in_c0.y, anchor_center_pos.x, anchor_center_pos.y);
|
||||
|
||||
}
|
||||
|
||||
// visu - feature
|
||||
//cv::resize(dottedFrame, dottedFrame, cv::Size(dottedFrame.cols*0.2, dottedFrame.rows*0.2));
|
||||
/*if(cam0.featureVisu.empty())
|
||||
cam0.featureVisu = dottedFrame.clone();
|
||||
else
|
||||
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
|
||||
|
||||
|
||||
// patches visualization
|
||||
int N = sqrt(anchorPatch_3d.size());
|
||||
int scale = 30;
|
||||
cv::Mat irradianceFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
|
||||
cv::resize(irradianceFrame, irradianceFrame, cv::Size(), rescale, rescale);
|
||||
|
||||
// irradiance grid anchor
|
||||
std::stringstream namer;
|
||||
namer << "anchor";
|
||||
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 25),
|
||||
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
|
||||
|
||||
|
||||
for(int i = 0; i<N; i++)
|
||||
for(int j = 0; j<N; j++)
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(30+scale*(i+1), 30+scale*j),
|
||||
cv::Point(30+scale*i, 30+scale*(j+1)),
|
||||
cv::Scalar(anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255, anchorPatch[i*N+j]*255),
|
||||
CV_FILLED);
|
||||
|
||||
// irradiance grid projection
|
||||
|
||||
namer.str(std::string());
|
||||
namer << "projection";
|
||||
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 45+scale*N),
|
||||
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
|
||||
|
||||
for(int i = 0; i<N; i++)
|
||||
for(int j = 0; j<N ; j++)
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(30+scale*(i+1), 50+scale*(N+j)),
|
||||
cv::Point(30+scale*(i), 50+scale*(N+j+1)),
|
||||
cv::Scalar(projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255, projectionPatch[i*N+j]*255),
|
||||
CV_FILLED);
|
||||
|
||||
// true irradiance at feature
|
||||
// get current observation
|
||||
|
||||
namer.str(std::string());
|
||||
namer << "feature";
|
||||
cv::putText(irradianceFrame, namer.str() , cvPoint(30, 65+scale*2*N),
|
||||
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
|
||||
|
||||
cv::Point2f p_f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
|
||||
// move to real pixels
|
||||
p_f = image_handler::distortPoint(p_f, cam0.intrinsics, cam0.distortion_model, cam0.distortion_coeffs);
|
||||
|
||||
for(int i = 0; i<N; i++)
|
||||
{
|
||||
for(int j = 0; j<N ; j++)
|
||||
{
|
||||
float irr = PixelIrradiance(cv::Point2f(p_f.x + (i-(N-1)/2), p_f.y + (j-(N-1)/2)), current_image);
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(30+scale*(i+1), 70+scale*(2*N+j)),
|
||||
cv::Point(30+scale*(i), 70+scale*(2*N+j+1)),
|
||||
cv::Scalar(irr*255, irr*255, irr*255),
|
||||
CV_FILLED);
|
||||
}
|
||||
}
|
||||
|
||||
// residual grid projection, positive - red, negative - blue colored
|
||||
namer.str(std::string());
|
||||
namer << "residual";
|
||||
cv::putText(irradianceFrame, namer.str() , cvPoint(30+scale*N, scale*N/2-5),
|
||||
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(0,0,0), 1, CV_AA);
|
||||
|
||||
for(int i = 0; i<N; i++)
|
||||
for(int j = 0; j<N; j++)
|
||||
if(photo_r(i*N+j)>0)
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)),
|
||||
cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)),
|
||||
cv::Scalar(255 - photo_r(i*N+j)*255, 255 - photo_r(i*N+j)*255, 255),
|
||||
CV_FILLED);
|
||||
else
|
||||
cv::rectangle(irradianceFrame,
|
||||
cv::Point(40+scale*(N+i+1), 15+scale*(N/2+j)),
|
||||
cv::Point(40+scale*(N+i), 15+scale*(N/2+j+1)),
|
||||
cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255),
|
||||
CV_FILLED);
|
||||
|
||||
// gradient arrow
|
||||
/*
|
||||
cv::arrowedLine(irradianceFrame,
|
||||
cv::Point(30+scale*(N/2 +0.5), 50+scale*(N+(N/2)+0.5)),
|
||||
cv::Point(30+scale*(N/2+0.5)+scale*gradientVector.x, 50+scale*(N+(N/2)+0.5)+scale*gradientVector.y),
|
||||
cv::Scalar(100, 0, 255),
|
||||
1);
|
||||
*/
|
||||
|
||||
// residual gradient direction
|
||||
cv::arrowedLine(irradianceFrame,
|
||||
cv::Point(40+scale*(N+N/2+0.5), 15+scale*((N-0.5))),
|
||||
cv::Point(40+scale*(N+N/2+0.5)+scale*residualVector.x, 15+scale*(N-0.5)+scale*residualVector.y),
|
||||
cv::Scalar(0, 255, 175),
|
||||
3);
|
||||
|
||||
|
||||
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
|
||||
|
||||
/*
|
||||
// visualize position of used observations and resulting feature position
|
||||
cv::Mat positionFrame(anchorImage.size(), CV_8UC3, cv::Scalar(255, 240, 255));
|
||||
cv::resize(positionFrame, positionFrame, cv::Size(), rescale, rescale);
|
||||
|
||||
|
||||
// draw world zero
|
||||
cv::line(positionFrame,
|
||||
cv::Point(20,20),
|
||||
cv::Point(20,30),
|
||||
cv::Scalar(0,0,255),
|
||||
CV_FILLED);
|
||||
|
||||
cv::line(positionFrame,
|
||||
cv::Point(20,20),
|
||||
cv::Point(30,20),
|
||||
cv::Scalar(255,0,0),
|
||||
CV_FILLED);
|
||||
// for every observation, get cam state
|
||||
for(auto obs : observations)
|
||||
{
|
||||
cv::line(positionFrame,
|
||||
cv::Point(20,20),
|
||||
cv::Point(30,20),
|
||||
cv::Scalar(255,0,0),
|
||||
CV_FILLED);
|
||||
}
|
||||
|
||||
// draw, x y position and arrow with direction - write z next to it
|
||||
|
||||
cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
|
||||
|
||||
|
||||
cv::hconcat(cam0.featureVisu, positionFrame, cam0.featureVisu);
|
||||
*/
|
||||
// write feature position
|
||||
std::stringstream pos_s;
|
||||
pos_s << "u: " << observations.begin()->second(0) << " v: " << observations.begin()->second(1);
|
||||
cv::putText(cam0.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30),
|
||||
cv::FONT_HERSHEY_COMPLEX_SMALL, 0.8, cvScalar(200,200,250), 1, CV_AA);
|
||||
// create line?
|
||||
|
||||
//save image
|
||||
|
||||
std::stringstream loc;
|
||||
// loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
|
||||
//cv::imwrite(loc.str(), cam0.featureVisu);
|
||||
|
||||
cv::imshow("patch", cam0.featureVisu);
|
||||
cvWaitKey(0);
|
||||
}
|
||||
|
||||
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
|
||||
{
|
||||
return (float)image.at<uint8_t>(pose.x, pose.y);
|
||||
|
||||
return ((float)image.at<uint8_t>(pose.y, pose.x))/255;
|
||||
}
|
||||
|
||||
cv::Point2f Feature::pixelDistanceAt(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
const CameraCalibration& cam,
|
||||
Eigen::Vector3d& in_p) const
|
||||
{
|
||||
|
||||
cv::Point2f cam_p = projectPositionToCamera(cam_state, cam_state_id, cam, in_p);
|
||||
|
||||
// create vector of patch in pixel plane
|
||||
std::vector<cv::Point2f> surroundingPoints;
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x+1, cam_p.y));
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x-1, cam_p.y));
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y+1));
|
||||
surroundingPoints.push_back(cv::Point2f(cam_p.x, cam_p.y-1));
|
||||
|
||||
std::vector<cv::Point2f> pure;
|
||||
image_handler::undistortPoints(surroundingPoints,
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs,
|
||||
pure);
|
||||
|
||||
// returns the absolute pixel distance at pixels one metres away
|
||||
cv::Point2f distance(fabs(pure[0].x - pure[1].x), fabs(pure[2].y - pure[3].y));
|
||||
|
||||
return distance;
|
||||
}
|
||||
|
||||
|
||||
|
||||
cv::Point2f Feature::projectPositionToCamera(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
@ -477,8 +914,7 @@ cv::Point2f Feature::projectPositionToCamera(
|
||||
return my_p;
|
||||
}
|
||||
|
||||
Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
|
||||
const CameraCalibration& cam)
|
||||
Eigen::Vector3d Feature::AnchorPixelToPosition(cv::Point2f in_p, const CameraCalibration& cam)
|
||||
{
|
||||
// use undistorted position of point of interest
|
||||
// project it back into 3D space using pinhole model
|
||||
@ -491,13 +927,10 @@ Eigen::Vector3d Feature::projectPixelToPosition(cv::Point2f in_p,
|
||||
}
|
||||
|
||||
//@test center projection must always be initial feature projection
|
||||
bool Feature::initializeAnchor(
|
||||
const CameraCalibration& cam)
|
||||
bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
|
||||
{
|
||||
|
||||
//initialize patch Size
|
||||
//TODO make N size a ros parameter
|
||||
int N = 3;
|
||||
int n = (int)(N-1)/2;
|
||||
|
||||
auto anchor = observations.begin();
|
||||
@ -516,54 +949,194 @@ bool Feature::initializeAnchor(
|
||||
int count = 0;
|
||||
|
||||
// get feature in undistorted pixel space
|
||||
// this only reverts from 'pure' space into undistorted pixel space using camera matrix
|
||||
cv::Point2f und_pix_p = image_handler::distortPoint(cv::Point2f(u, v),
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
0);
|
||||
cam.distortion_coeffs);
|
||||
// create vector of patch in pixel plane
|
||||
std::vector<cv::Point2f> und_pix_v;
|
||||
for(double u_run = -n; u_run <= n; u_run++)
|
||||
for(double v_run = -n; v_run <= n; v_run++)
|
||||
und_pix_v.push_back(cv::Point2f(und_pix_p.x-u_run, und_pix_p.y-v_run));
|
||||
|
||||
anchorPatch_real.push_back(cv::Point2f(und_pix_p.x+u_run, und_pix_p.y+v_run));
|
||||
|
||||
|
||||
//create undistorted pure points
|
||||
std::vector<cv::Point2f> und_v;
|
||||
image_handler::undistortPoints(und_pix_v,
|
||||
image_handler::undistortPoints(anchorPatch_real,
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
0,
|
||||
und_v);
|
||||
//create distorted pixel points
|
||||
std::vector<cv::Point2f> vec = image_handler::distortPoints(und_v,
|
||||
cam.intrinsics,
|
||||
cam.distortion_model,
|
||||
cam.distortion_coeffs);
|
||||
cam.distortion_coeffs,
|
||||
anchorPatch_ideal);
|
||||
|
||||
|
||||
// save anchor position for later visualisaztion
|
||||
anchor_center_pos = vec[4];
|
||||
for(auto point : vec)
|
||||
{
|
||||
anchor_center_pos = anchorPatch_real[(N*N-1)/2];
|
||||
|
||||
// save true pixel Patch position
|
||||
for(auto point : anchorPatch_real)
|
||||
if(point.x - n < 0 || point.x + n >= cam.resolution(0) || point.y - n < 0 || point.y + n >= cam.resolution(1))
|
||||
return false;
|
||||
}
|
||||
for(auto point : vec)
|
||||
anchorPatch.push_back((double)anchorImage.at<uint8_t>((int)point.x,(int)point.y));
|
||||
|
||||
// project patch pixel to 3D space
|
||||
for(auto point : und_v)
|
||||
anchorPatch_3d.push_back(projectPixelToPosition(point, cam));
|
||||
for(auto point : anchorPatch_real)
|
||||
anchorPatch.push_back(PixelIrradiance(point, anchorImage));
|
||||
|
||||
// project patch pixel to 3D space in camera coordinate system
|
||||
for(auto point : anchorPatch_ideal)
|
||||
anchorPatch_3d.push_back(AnchorPixelToPosition(point, cam));
|
||||
|
||||
is_anchored = true;
|
||||
return true;
|
||||
}
|
||||
|
||||
bool Feature::initializePosition(
|
||||
const CamStateServer& cam_states) {
|
||||
bool Feature::initializeRho(const CamStateServer& cam_states) {
|
||||
|
||||
// Organize camera poses and feature observations properly.
|
||||
std::vector<Eigen::Isometry3d,
|
||||
Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
|
||||
std::vector<Eigen::Vector2d,
|
||||
Eigen::aligned_allocator<Eigen::Vector2d> > measurements(0);
|
||||
|
||||
for (auto& m : observations) {
|
||||
auto cam_state_iter = cam_states.find(m.first);
|
||||
if (cam_state_iter == cam_states.end()) continue;
|
||||
|
||||
// Add the measurement.
|
||||
measurements.push_back(m.second.head<2>());
|
||||
measurements.push_back(m.second.tail<2>());
|
||||
|
||||
// This camera pose will take a vector from this camera frame
|
||||
// to the world frame.
|
||||
Eigen::Isometry3d cam0_pose;
|
||||
cam0_pose.linear() = quaternionToRotation(
|
||||
cam_state_iter->second.orientation).transpose();
|
||||
cam0_pose.translation() = cam_state_iter->second.position;
|
||||
|
||||
Eigen::Isometry3d cam1_pose;
|
||||
cam1_pose = cam0_pose * CAMState::T_cam0_cam1.inverse();
|
||||
|
||||
cam_poses.push_back(cam0_pose);
|
||||
cam_poses.push_back(cam1_pose);
|
||||
}
|
||||
|
||||
// All camera poses should be modified such that it takes a
|
||||
// vector from the first camera frame in the buffer to this
|
||||
// camera frame.
|
||||
Eigen::Isometry3d T_c0_w = cam_poses[0];
|
||||
T_anchor_w = T_c0_w;
|
||||
for (auto& pose : cam_poses)
|
||||
pose = pose.inverse() * T_c0_w;
|
||||
|
||||
// Generate initial guess
|
||||
double initial_depth = 0;
|
||||
initial_depth = generateInitialDepth(cam_poses[cam_poses.size()-1], measurements[0],
|
||||
measurements[measurements.size()-1]);
|
||||
|
||||
double solution = 1.0/initial_depth;
|
||||
|
||||
// Apply Levenberg-Marquart method to solve for the 3d position.
|
||||
double lambda = optimization_config.initial_damping;
|
||||
int inner_loop_cntr = 0;
|
||||
int outer_loop_cntr = 0;
|
||||
bool is_cost_reduced = false;
|
||||
double delta_norm = 0;
|
||||
|
||||
// Compute the initial cost.
|
||||
double total_cost = 0.0;
|
||||
for (int i = 0; i < cam_poses.size(); ++i) {
|
||||
double this_cost = 0.0;
|
||||
Rhocost(cam_poses[i], solution, measurements[0], measurements[i], this_cost);
|
||||
total_cost += this_cost;
|
||||
}
|
||||
|
||||
// Outer loop.
|
||||
do {
|
||||
Eigen::Matrix<double, 1, 1> A = Eigen::Matrix<double, 1, 1>::Zero();
|
||||
Eigen::Matrix<double, 1, 1> b = Eigen::Matrix<double, 1, 1>::Zero();
|
||||
|
||||
for (int i = 0; i < cam_poses.size(); ++i) {
|
||||
Eigen::Matrix<double, 2, 1> J;
|
||||
Eigen::Vector2d r;
|
||||
double w;
|
||||
|
||||
RhoJacobian(cam_poses[i], solution, measurements[0], measurements[i], J, r, w);
|
||||
|
||||
if (w == 1) {
|
||||
A += J.transpose() * J;
|
||||
b += J.transpose() * r;
|
||||
} else {
|
||||
double w_square = w * w;
|
||||
A += w_square * J.transpose() * J;
|
||||
b += w_square * J.transpose() * r;
|
||||
}
|
||||
}
|
||||
|
||||
// Inner loop.
|
||||
// Solve for the delta that can reduce the total cost.
|
||||
do {
|
||||
Eigen::Matrix<double, 1, 1> damper = lambda*Eigen::Matrix<double, 1, 1>::Identity();
|
||||
Eigen::Matrix<double, 1, 1> delta = (A+damper).ldlt().solve(b);
|
||||
double new_solution = solution - delta(0,0);
|
||||
delta_norm = delta.norm();
|
||||
|
||||
double new_cost = 0.0;
|
||||
for (int i = 0; i < cam_poses.size(); ++i) {
|
||||
double this_cost = 0.0;
|
||||
Rhocost(cam_poses[i], new_solution, measurements[0], measurements[i], this_cost);
|
||||
new_cost += this_cost;
|
||||
}
|
||||
|
||||
if (new_cost < total_cost) {
|
||||
is_cost_reduced = true;
|
||||
solution = new_solution;
|
||||
total_cost = new_cost;
|
||||
lambda = lambda/10 > 1e-10 ? lambda/10 : 1e-10;
|
||||
} else {
|
||||
is_cost_reduced = false;
|
||||
lambda = lambda*10 < 1e12 ? lambda*10 : 1e12;
|
||||
}
|
||||
|
||||
} while (inner_loop_cntr++ <
|
||||
optimization_config.inner_loop_max_iteration && !is_cost_reduced);
|
||||
|
||||
inner_loop_cntr = 0;
|
||||
|
||||
} while (outer_loop_cntr++ <
|
||||
optimization_config.outer_loop_max_iteration &&
|
||||
delta_norm > optimization_config.estimation_precision);
|
||||
|
||||
// Covert the feature position from inverse depth
|
||||
// representation to its 3d coordinate.
|
||||
Eigen::Vector3d final_position(measurements[0](0)/solution,
|
||||
measurements[0](1)/solution, 1.0/solution);
|
||||
|
||||
// Check if the solution is valid. Make sure the feature
|
||||
// is in front of every camera frame observing it.
|
||||
bool is_valid_solution = true;
|
||||
for (const auto& pose : cam_poses) {
|
||||
Eigen::Vector3d position =
|
||||
pose.linear()*final_position + pose.translation();
|
||||
if (position(2) <= 0) {
|
||||
is_valid_solution = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
//save inverse depth distance from camera
|
||||
anchor_rho = solution;
|
||||
|
||||
// Convert the feature position to the world frame.
|
||||
position = T_c0_w.linear()*final_position + T_c0_w.translation();
|
||||
|
||||
if (is_valid_solution)
|
||||
is_initialized = true;
|
||||
|
||||
return is_valid_solution;
|
||||
}
|
||||
|
||||
|
||||
bool Feature::initializePosition(const CamStateServer& cam_states) {
|
||||
|
||||
// Organize camera poses and feature observations properly.
|
||||
|
||||
std::vector<Eigen::Isometry3d,
|
||||
Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
|
||||
std::vector<Eigen::Vector2d,
|
||||
@ -701,6 +1274,7 @@ bool Feature::initializePosition(
|
||||
|
||||
//save inverse depth distance from camera
|
||||
anchor_rho = solution(2);
|
||||
std::cout << "from feature: " << anchor_rho << std::endl;
|
||||
|
||||
// Convert the feature position to the world frame.
|
||||
position = T_c0_w.linear()*final_position + T_c0_w.translation();
|
||||
@ -710,6 +1284,7 @@ bool Feature::initializePosition(
|
||||
|
||||
return is_valid_solution;
|
||||
}
|
||||
|
||||
} // namespace msckf_vio
|
||||
|
||||
#endif // MSCKF_VIO_FEATURE_H
|
||||
|
@ -36,6 +36,15 @@ cv::Point2f distortPoint(
|
||||
const cv::Vec4d& intrinsics,
|
||||
const std::string& distortion_model,
|
||||
const cv::Vec4d& distortion_coeffs);
|
||||
|
||||
void undistortPoint(
|
||||
const cv::Point2f& pt_in,
|
||||
const cv::Vec4d& intrinsics,
|
||||
const std::string& distortion_model,
|
||||
const cv::Vec4d& distortion_coeffs,
|
||||
cv::Point2f& pt_out,
|
||||
const cv::Matx33d &rectification_matrix = cv::Matx33d::eye(),
|
||||
const cv::Vec4d &new_intrinsics = cv::Vec4d(1,1,0,0));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
@ -43,6 +43,50 @@ inline void quaternionNormalize(Eigen::Vector4d& q) {
|
||||
return;
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief invert rotation of passed quaternion through conjugation
|
||||
* and return conjugation
|
||||
*/
|
||||
inline Eigen::Vector4d quaternionConjugate(Eigen::Vector4d& q)
|
||||
{
|
||||
Eigen::Vector4d p;
|
||||
p(0) = -q(0);
|
||||
p(1) = -q(1);
|
||||
p(2) = -q(2);
|
||||
p(3) = q(3);
|
||||
quaternionNormalize(p);
|
||||
return p;
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief converts a vector4 to a vector3, dropping (3)
|
||||
* this is typically used to get the vector part of a quaternion
|
||||
for eq small angle approximation
|
||||
*/
|
||||
inline Eigen::Vector3d v4tov3(const Eigen::Vector4d& q)
|
||||
{
|
||||
Eigen::Vector3d p;
|
||||
p(0) = q(0);
|
||||
p(1) = q(1);
|
||||
p(2) = q(2);
|
||||
return p;
|
||||
}
|
||||
|
||||
/*
|
||||
* @brief Perform q1 * q2
|
||||
*/
|
||||
|
||||
inline Eigen::Vector4d QtoV(const Eigen::Quaterniond& q)
|
||||
{
|
||||
Eigen::Vector4d p;
|
||||
p(0) = q.x();
|
||||
p(1) = q.y();
|
||||
p(2) = q.z();
|
||||
p(3) = q.w();
|
||||
return p;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* @brief Perform q1 * q2
|
||||
*/
|
||||
|
@ -107,6 +107,15 @@ class MsckfVio {
|
||||
*/
|
||||
void imuCallback(const sensor_msgs::ImuConstPtr& msg);
|
||||
|
||||
/*
|
||||
* @brief truthCallback
|
||||
* Callback function for ground truth navigation information
|
||||
* @param TransformStamped msg
|
||||
*/
|
||||
void truthCallback(
|
||||
const geometry_msgs::TransformStampedPtr& msg);
|
||||
|
||||
|
||||
/*
|
||||
* @brief imageCallback
|
||||
* Callback function for feature measurements
|
||||
@ -144,11 +153,26 @@ class MsckfVio {
|
||||
bool resetCallback(std_srvs::Trigger::Request& req,
|
||||
std_srvs::Trigger::Response& res);
|
||||
|
||||
// Saves the exposure time and the camera measurementes
|
||||
void manageMovingWindow(
|
||||
const sensor_msgs::ImageConstPtr& cam0_img,
|
||||
const sensor_msgs::ImageConstPtr& cam1_img,
|
||||
const CameraMeasurementConstPtr& feature_msg);
|
||||
|
||||
|
||||
void calcErrorState();
|
||||
|
||||
// Debug related Functions
|
||||
// Propagate the true state
|
||||
void batchTruthProcessing(
|
||||
const double& time_bound);
|
||||
|
||||
|
||||
void processTruthtoIMU(const double& time,
|
||||
const Eigen::Vector4d& m_rot,
|
||||
const Eigen::Vector3d& m_trans);
|
||||
|
||||
|
||||
// Filter related functions
|
||||
// Propogate the state
|
||||
void batchImuProcessing(
|
||||
@ -160,16 +184,20 @@ class MsckfVio {
|
||||
const Eigen::Vector3d& gyro,
|
||||
const Eigen::Vector3d& acc);
|
||||
|
||||
// groundtruth state augmentation
|
||||
void truePhotometricStateAugmentation(const double& time);
|
||||
|
||||
// Measurement update
|
||||
void stateAugmentation(const double& time);
|
||||
void PhotometricStateAugmentation(const double& time);
|
||||
void addFeatureObservations(const CameraMeasurementConstPtr& msg);
|
||||
// This function is used to compute the measurement Jacobian
|
||||
// for a single feature observed at a single camera frame.
|
||||
void measurementJacobian(const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
Eigen::Matrix<double, 4, 6>& H_x,
|
||||
Eigen::Matrix<double, 4, 3>& H_f,
|
||||
Eigen::Vector4d& r);
|
||||
Eigen::Matrix<double, 2, 6>& H_x,
|
||||
Eigen::Matrix<double, 2, 3>& H_f,
|
||||
Eigen::Vector2d& r);
|
||||
// This function computes the Jacobian of all measurements viewed
|
||||
// in the given camera states of this feature.
|
||||
void featureJacobian(const FeatureIDType& feature_id,
|
||||
@ -180,9 +208,9 @@ class MsckfVio {
|
||||
void PhotometricMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
Eigen::Matrix<double, 4, 6>& H_x,
|
||||
Eigen::Matrix<double, 4, 3>& H_f,
|
||||
Eigen::Vector4d& r);
|
||||
Eigen::MatrixXd& H_x,
|
||||
Eigen::MatrixXd& H_y,
|
||||
Eigen::VectorXd& r);
|
||||
|
||||
void PhotometricFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
@ -200,11 +228,32 @@ class MsckfVio {
|
||||
// Reset the system online if the uncertainty is too large.
|
||||
void onlineReset();
|
||||
|
||||
// Photometry flag
|
||||
bool PHOTOMETRIC;
|
||||
|
||||
// debug flag
|
||||
bool PRINTIMAGES;
|
||||
bool GROUNDTRUTH;
|
||||
|
||||
bool nan_flag;
|
||||
bool play;
|
||||
double last_time_bound;
|
||||
|
||||
// Patch size for Photometry
|
||||
int N;
|
||||
|
||||
// Chi squared test table.
|
||||
static std::map<int, double> chi_squared_test_table;
|
||||
|
||||
// State vector
|
||||
StateServer state_server;
|
||||
|
||||
// Ground truth state vector
|
||||
StateServer true_state_server;
|
||||
|
||||
// error state based on ground truth
|
||||
StateServer err_state_server;
|
||||
|
||||
// Maximum number of camera states
|
||||
int max_cam_state_size;
|
||||
|
||||
@ -216,6 +265,8 @@ class MsckfVio {
|
||||
// transfer delay between IMU and Image messages.
|
||||
std::vector<sensor_msgs::Imu> imu_msg_buffer;
|
||||
|
||||
// Ground Truth message data
|
||||
std::vector<geometry_msgs::TransformStamped> truth_msg_buffer;
|
||||
// Moving Window buffer
|
||||
movingWindow cam0_moving_window;
|
||||
movingWindow cam1_moving_window;
|
||||
@ -224,6 +275,8 @@ class MsckfVio {
|
||||
CameraCalibration cam0;
|
||||
CameraCalibration cam1;
|
||||
|
||||
// covariance data
|
||||
double irradiance_frame_bias;
|
||||
|
||||
|
||||
ros::Time image_save_time;
|
||||
@ -255,7 +308,9 @@ class MsckfVio {
|
||||
|
||||
// Subscribers and publishers
|
||||
ros::Subscriber imu_sub;
|
||||
ros::Subscriber truth_sub;
|
||||
ros::Publisher odom_pub;
|
||||
ros::Publisher marker_pub;
|
||||
ros::Publisher feature_pub;
|
||||
tf::TransformBroadcaster tf_pub;
|
||||
ros::ServiceServer reset_srv;
|
||||
@ -287,6 +342,9 @@ class MsckfVio {
|
||||
ros::Publisher mocap_odom_pub;
|
||||
geometry_msgs::TransformStamped raw_mocap_odom_msg;
|
||||
Eigen::Isometry3d mocap_initial_frame;
|
||||
|
||||
Eigen::Vector4d mocap_initial_orientation;
|
||||
Eigen::Vector3d mocap_initial_position;
|
||||
};
|
||||
|
||||
typedef MsckfVio::Ptr MsckfVioPtr;
|
||||
|
75
launch/msckf_vio_debug_tum.launch
Normal file
75
launch/msckf_vio_debug_tum.launch
Normal file
@ -0,0 +1,75 @@
|
||||
<launch>
|
||||
|
||||
<arg name="robot" default="firefly_sbx"/>
|
||||
<arg name="fixed_frame_id" default="world"/>
|
||||
<arg name="calibration_file"
|
||||
default="$(find msckf_vio)/config/camchain-imucam-tum.yaml"/>
|
||||
|
||||
<!-- Image Processor Nodelet -->
|
||||
<include file="$(find msckf_vio)/launch/image_processor_tum.launch">
|
||||
<arg name="robot" value="$(arg robot)"/>
|
||||
<arg name="calibration_file" value="$(arg calibration_file)"/>
|
||||
</include>
|
||||
|
||||
<!-- Msckf Vio Nodelet -->
|
||||
<group ns="$(arg robot)">
|
||||
<node pkg="nodelet" type="nodelet" name="vio"
|
||||
args='standalone msckf_vio/MsckfVioNodelet'
|
||||
output="screen"
|
||||
launch-prefix="xterm -e gdb --args">
|
||||
|
||||
<!-- Photometry Flag-->
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="7"/>
|
||||
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
<param name="publish_tf" value="true"/>
|
||||
<param name="frame_rate" value="20"/>
|
||||
<param name="fixed_frame_id" value="$(arg fixed_frame_id)"/>
|
||||
<param name="child_frame_id" value="odom"/>
|
||||
<param name="max_cam_state_size" value="20"/>
|
||||
<param name="position_std_threshold" value="8.0"/>
|
||||
|
||||
<param name="rotation_threshold" value="0.2618"/>
|
||||
<param name="translation_threshold" value="0.4"/>
|
||||
<param name="tracking_rate_threshold" value="0.5"/>
|
||||
|
||||
<!-- Feature optimization config -->
|
||||
<param name="feature/config/translation_threshold" value="-1.0"/>
|
||||
|
||||
<!-- These values should be standard deviation -->
|
||||
<param name="noise/gyro" value="0.005"/>
|
||||
<param name="noise/acc" value="0.05"/>
|
||||
<param name="noise/gyro_bias" value="0.001"/>
|
||||
<param name="noise/acc_bias" value="0.01"/>
|
||||
<param name="noise/feature" value="0.035"/>
|
||||
|
||||
<param name="initial_state/velocity/x" value="0.0"/>
|
||||
<param name="initial_state/velocity/y" value="0.0"/>
|
||||
<param name="initial_state/velocity/z" value="0.0"/>
|
||||
|
||||
<!-- These values should be covariance -->
|
||||
<param name="initial_covariance/velocity" value="0.25"/>
|
||||
<param name="initial_covariance/gyro_bias" value="0.01"/>
|
||||
<param name="initial_covariance/acc_bias" value="0.01"/>
|
||||
<param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
|
||||
<param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
|
||||
<param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
|
||||
|
||||
<remap from="~imu" to="/imu0"/>
|
||||
<remap from="~cam0_image" to="/cam0/image_raw"/>
|
||||
<remap from="~cam1_image" to="/cam1/image_raw"/>
|
||||
|
||||
<remap from="~features" to="image_processor/features"/>
|
||||
|
||||
</node>
|
||||
</group>
|
||||
|
||||
</launch>
|
@ -17,6 +17,14 @@
|
||||
args='standalone msckf_vio/MsckfVioNodelet'
|
||||
output="screen">
|
||||
|
||||
<!-- Photometry Flag-->
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="1"/>
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
@ -51,8 +59,10 @@
|
||||
<param name="initial_covariance/acc_bias" value="0.01"/>
|
||||
<param name="initial_covariance/extrinsic_rotation_cov" value="3.0462e-4"/>
|
||||
<param name="initial_covariance/extrinsic_translation_cov" value="2.5e-5"/>
|
||||
<param name="initial_covariance/irradiance_frame_bias" value="0.1"/>
|
||||
|
||||
<remap from="~imu" to="/imu0"/>
|
||||
<remap from="~ground_truth" to="/vrpn_client/raw_transform"/>
|
||||
<remap from="~cam0_image" to="/cam0/image_raw"/>
|
||||
<remap from="~cam1_image" to="/cam1/image_raw"/>
|
||||
|
||||
|
@ -18,6 +18,7 @@
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>eigen_conversions</depend>
|
||||
<depend>tf_conversions</depend>
|
||||
<depend>random_numbers</depend>
|
||||
|
@ -14,6 +14,48 @@ namespace msckf_vio {
|
||||
namespace image_handler {
|
||||
|
||||
|
||||
|
||||
void undistortPoint(
|
||||
const cv::Point2f& pt_in,
|
||||
const cv::Vec4d& intrinsics,
|
||||
const std::string& distortion_model,
|
||||
const cv::Vec4d& distortion_coeffs,
|
||||
cv::Point2f& pt_out,
|
||||
const cv::Matx33d &rectification_matrix,
|
||||
const cv::Vec4d &new_intrinsics) {
|
||||
|
||||
|
||||
std::vector<cv::Point2f> pts_in;
|
||||
std::vector<cv::Point2f> pts_out;
|
||||
pts_in.push_back(pt_in);
|
||||
if (pts_in.size() == 0) return;
|
||||
|
||||
const cv::Matx33d K(
|
||||
intrinsics[0], 0.0, intrinsics[2],
|
||||
0.0, intrinsics[1], intrinsics[3],
|
||||
0.0, 0.0, 1.0);
|
||||
|
||||
const cv::Matx33d K_new(
|
||||
new_intrinsics[0], 0.0, new_intrinsics[2],
|
||||
0.0, new_intrinsics[1], new_intrinsics[3],
|
||||
0.0, 0.0, 1.0);
|
||||
|
||||
if (distortion_model == "radtan") {
|
||||
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
|
||||
rectification_matrix, K_new);
|
||||
} else if (distortion_model == "equidistant") {
|
||||
cv::fisheye::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
|
||||
rectification_matrix, K_new);
|
||||
} else {
|
||||
ROS_WARN_ONCE("The model %s is unrecognized, use radtan instead...",
|
||||
distortion_model.c_str());
|
||||
cv::undistortPoints(pts_in, pts_out, K, distortion_coeffs,
|
||||
rectification_matrix, K_new);
|
||||
}
|
||||
pt_out = pts_out[0];
|
||||
return;
|
||||
}
|
||||
|
||||
void undistortPoints(
|
||||
const std::vector<cv::Point2f>& pts_in,
|
||||
const cv::Vec4d& intrinsics,
|
||||
|
File diff suppressed because it is too large
Load Diff
Reference in New Issue
Block a user