raphael/msckf_vio
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

added direct levenberg marqhart estimation for rho, was previously calculated from feature position

Browse Source
This commit is contained in:
Raphael Maenle
2019-06-04 17:38:11 +02:00
parent 2a16fb2fc5
commit 5f6bcd1784
3 changed files with 296 additions and 40 deletions
Download Patch File Download Diff File Expand all files Collapse all files

265
include/msckf_vio/feature.hpp
View File

@ -70,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
@ -82,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
@ -97,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.
@ -194,7 +210,7 @@ bool MarkerGeneration(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
const std::vector<double> photo_r,
const Eigen::VectorXd& photo_r,
std::stringstream& ss,
cv::Point2f gradientVector,
cv::Point2f residualVector) const;
@ -263,6 +279,26 @@ 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,
@ -275,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);
@ -287,6 +323,43 @@ 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
{
// Compute hi1, hi2, and hi3 as Equation (37).
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,
@ -326,9 +399,9 @@ 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);
@ -343,6 +416,15 @@ 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;
@ -405,16 +487,11 @@ else if(type == "Sobel_y")
double delta = 0;
int offs = (int)(kernel.rows()-1)/2;
for(int i = 0; i < kernel.rows(); i++){
for(int i = 0; i < kernel.rows(); i++)
for(int j = 0; j < kernel.cols(); j++)
{
std::cout << "i: " << i << ":" << "j: " << j << ":" << kernel(i,j) << std::endl;
std::cout <<"pose: " << pose.y+i-offs << " : " << pose.x+j-offs << std::endl;
delta += ((float)frame.at<uint8_t>(pose.y+i-offs , pose.x+j-offs))/255 * (float)kernel(i,j);
}
}
std::cout << "delta " << delta << std::endl;
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(
@ -572,7 +649,7 @@ bool Feature::VisualizePatch(
const CAMState& cam_state,
const StateIDType& cam_state_id,
CameraCalibration& cam0,
const std::vector<double> photo_r,
const Eigen::VectorXd& photo_r,
std::stringstream& ss,
cv::Point2f gradientVector,
cv::Point2f residualVector) const
@ -685,17 +762,17 @@ bool Feature::VisualizePatch(
for(int i = 0; i<N; i++)
for(int j = 0; j<N; j++)
if(photo_r[i*N+j]>0)
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::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::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255),
CV_FILLED);
// gradient arrow
@ -911,9 +988,160 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
return true;
}
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.
initializeRho(cam_states);
std::cout << "init rho is: " << anchor_rho << std::endl;
std::vector<Eigen::Isometry3d,
Eigen::aligned_allocator<Eigen::Isometry3d> > cam_poses(0);
std::vector<Eigen::Vector2d,
@ -1051,6 +1279,7 @@ bool Feature::initializePosition(const CamStateServer& cam_states) {
//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();