added direct levenberg marqhart estimation for rho, was previously calculated from feature position
This commit is contained in:
parent
2a16fb2fc5
commit
5f6bcd1784
@ -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();
|
||||
|
||||
@ -18,13 +18,13 @@
|
||||
output="screen">
|
||||
|
||||
<!-- Photometry Flag-->
|
||||
<param name="PHOTOMETRIC" value="true"/>
|
||||
<param name="PHOTOMETRIC" value="false"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="1"/>
|
||||
<param name="patch_size_n" value="3"/>
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
|
||||
@ -1240,9 +1240,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
//photometric observation
|
||||
std::vector<double> photo_z;
|
||||
|
||||
std::vector<double> photo_r;
|
||||
|
||||
VectorXd r_photo = VectorXd::Zero(N*N);
|
||||
// individual Jacobians
|
||||
Matrix<double, 1, 2> dI_dhj = Matrix<double, 1, 2>::Zero();
|
||||
Matrix<double, 2, 3> dh_dCpij = Matrix<double, 2, 3>::Zero();
|
||||
@ -1303,8 +1301,7 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
|
||||
|
||||
//calculate photom. residual
|
||||
photo_r.push_back((photo_z[count] - estimate_photo_z[count]));
|
||||
|
||||
r_photo(count) = photo_z[count] - estimate_photo_z[count];
|
||||
//cout << "residual: " << photo_r.back() << endl;
|
||||
|
||||
// add jacobians
|
||||
@ -1325,9 +1322,10 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
gradientVector.x += dx;
|
||||
gradientVector.y += dy;
|
||||
|
||||
residualVector.x += dx * photo_r[count];
|
||||
residualVector.y += dy * photo_r[count];
|
||||
res_sum += photo_r[count];
|
||||
|
||||
residualVector.x += dx * r_photo(count);
|
||||
residualVector.y += dy * r_photo(count);
|
||||
res_sum += r_photo(count);
|
||||
|
||||
//dh / d{}^Cp_{ij}
|
||||
dh_dCpij(0, 0) = 1 / p_c0(2);
|
||||
@ -1348,7 +1346,6 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
// d{}^Gp_P{ij} / \rho_i
|
||||
double rho = feature.anchor_rho;
|
||||
|
||||
// Isometry T_anchor_w takes a vector in anchor frame to world frame
|
||||
dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho*rho), feature.anchorPatch_ideal[count].y/(rho*rho), 1/(rho*rho));
|
||||
|
||||
@ -1363,6 +1360,12 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
H_plj = dI_dhj * dh_dXplj; // 1 x 6
|
||||
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
|
||||
|
||||
/*
|
||||
cout << endl;
|
||||
std::cout << H_plj << endl;
|
||||
cout << r_photo.segment(count, 1) << endl;
|
||||
std::cout << H_plj.colPivHouseholderQr().solve(r_photo.segment(count, 1)) << std::endl;
|
||||
*/
|
||||
H_rho.block<1, 1>(count, 0) = H_rhoj;
|
||||
H_pl.block<1, 6>(count, 0) = H_plj;
|
||||
H_pA.block<1, 6>(count, 0) = H_pAj;
|
||||
@ -1375,6 +1378,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
count++;
|
||||
}
|
||||
|
||||
/*
|
||||
std::cout << "\n\n frame change through patch" << std::endl;
|
||||
std::cout << H_pl << std::endl;
|
||||
std::cout << r_photo << std::endl;
|
||||
std::cout << endl;
|
||||
std::cout << H_pl.colPivHouseholderQr().solve(r_photo) << std::endl;
|
||||
*/
|
||||
|
||||
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
|
||||
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+state_server.cam_states.size()+1);
|
||||
|
||||
@ -1408,16 +1419,14 @@ void MsckfVio::PhotometricMeasurementJacobian(
|
||||
H_y = H_yl;
|
||||
|
||||
//TODO make this more fluent as well
|
||||
count = 0;
|
||||
for(auto data : photo_r)
|
||||
r[count++] = data;
|
||||
r = r_photo;
|
||||
|
||||
std::stringstream ss;
|
||||
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
feature.MarkerGeneration(marker_pub, state_server.cam_states);
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, photo_r, ss, gradientVector, residualVector);
|
||||
//feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss, gradientVector, residualVector);
|
||||
}
|
||||
|
||||
return;
|
||||
@ -1463,6 +1472,8 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
if (feature.observations.find(cam_id) ==
|
||||
feature.observations.end()) continue;
|
||||
|
||||
if (feature.observations.find(cam_id) == feature.observations.begin())
|
||||
continue;
|
||||
valid_cam_state_ids.push_back(cam_id);
|
||||
}
|
||||
|
||||
@ -1519,6 +1530,7 @@ void MsckfVio::PhotometricFeatureJacobian(
|
||||
{
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "Hxi\n" << H_xi << "ri\n" << r_i << "Hyi\n" << H_yi << endl;
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
@ -1601,6 +1613,13 @@ void MsckfVio::featureJacobian(
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
MatrixXd& H_x, VectorXd& r)
|
||||
{
|
||||
// stop playing bagfile if printing images
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
std::cout << "stopped playpack" << std::endl;
|
||||
nh.setParam("/play_bag", false);
|
||||
}
|
||||
|
||||
|
||||
const auto& feature = map_server[feature_id];
|
||||
|
||||
@ -1662,12 +1681,20 @@ void MsckfVio::featureJacobian(
|
||||
H_x = A.transpose() * H_xj;
|
||||
r = A.transpose() * r_j;
|
||||
|
||||
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
|
||||
// stop playing bagfile if printing images
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
|
||||
ofstream myfile;
|
||||
myfile.open ("/home/raphael/dev/MSCKF_ws/log.txt");
|
||||
myfile << "Hx\n" << H_x << "r\n" << r << "from residual estimated error state: " << H_x.colPivHouseholderQr().solve(r) << endl;
|
||||
myfile.close();
|
||||
cout << "---------- LOGGED -------- " << endl;
|
||||
std::cout << "stopped playpack" << std::endl;
|
||||
nh.setParam("/play_bag", true);
|
||||
}
|
||||
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user