restructured calcualtion of patches in code
This commit is contained in:
parent
58fe991647
commit
737c23f32a
@ -202,32 +202,56 @@ class MsckfVio {
|
||||
Eigen::Vector4d& r);
|
||||
// This function computes the Jacobian of all measurements viewed
|
||||
// in the given camera states of this feature.
|
||||
void featureJacobian(const FeatureIDType& feature_id,
|
||||
void featureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
|
||||
|
||||
void twodotMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
Eigen::MatrixXd& H_x, Eigen::MatrixXd& H_y, Eigen::VectorXd& r);
|
||||
|
||||
bool ConstructJacobians(
|
||||
Eigen::MatrixXd& H_rho,
|
||||
Eigen::MatrixXd& H_pl,
|
||||
Eigen::MatrixXd& H_pA,
|
||||
const Feature& feature,
|
||||
const StateIDType& cam_state_id,
|
||||
Eigen::MatrixXd& H_xl,
|
||||
Eigen::MatrixXd& H_yl);
|
||||
|
||||
bool PhotometricPatchPointResidual(
|
||||
const StateIDType& cam_state_id,
|
||||
const Feature& feature,
|
||||
Eigen::VectorXd& r);
|
||||
|
||||
bool PhotometricPatchPointJacobian(
|
||||
const CAMState& cam_state,
|
||||
const Feature& feature,
|
||||
Eigen::Vector3d point,
|
||||
int count,
|
||||
Eigen::Matrix<double, 1, 1>& H_rhoj,
|
||||
Eigen::Matrix<double, 1, 6>& H_plj,
|
||||
Eigen::Matrix<double, 1, 6>& H_pAj);
|
||||
|
||||
bool PhotometricMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
Eigen::MatrixXd& H_x,
|
||||
Eigen::MatrixXd& H_y,
|
||||
Eigen::VectorXd& r);
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
Eigen::MatrixXd& H_x,
|
||||
Eigen::MatrixXd& H_y,
|
||||
Eigen::VectorXd& r);
|
||||
|
||||
void twodotFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
|
||||
bool PhotometricFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
|
||||
void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
|
||||
void measurementUpdate(const Eigen::MatrixXd& H,
|
||||
@ -263,6 +287,13 @@ class MsckfVio {
|
||||
// Chi squared test table.
|
||||
static std::map<int, double> chi_squared_test_table;
|
||||
|
||||
double eval_time;
|
||||
|
||||
IMUState timed_old_imu_state;
|
||||
IMUState timed_old_true_state;
|
||||
|
||||
IMUState old_imu_state;
|
||||
IMUState old_true_state;
|
||||
|
||||
// change in position
|
||||
Eigen::Vector3d delta_position;
|
||||
|
||||
@ -18,14 +18,14 @@
|
||||
output="screen">
|
||||
|
||||
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
|
||||
<param name="FILTER" value="1"/>
|
||||
<param name="FILTER" value="0"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="StreamPause" value="true"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="5"/>
|
||||
<param name="patch_size_n" value="3"/>
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
|
||||
@ -223,8 +223,8 @@ void ImageProcessor::stereoCallback(
|
||||
|
||||
image_handler::undistortImage(cam0_curr_img_ptr->image, cam0_curr_img_ptr->image, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
|
||||
image_handler::undistortImage(cam1_curr_img_ptr->image, cam1_curr_img_ptr->image, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
|
||||
ROS_INFO("Publishing: %f",
|
||||
(ros::Time::now()-start_time).toSec());
|
||||
//ROS_INFO("Publishing: %f",
|
||||
// (ros::Time::now()-start_time).toSec());
|
||||
// Build the image pyramids once since they're used at multiple places
|
||||
createImagePyramids();
|
||||
|
||||
|
||||
@ -260,6 +260,7 @@ bool MsckfVio::createRosIO() {
|
||||
// activating bag playing parameter, for debugging
|
||||
nh.setParam("/play_bag", true);
|
||||
|
||||
eval_time = 0;
|
||||
odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 10);
|
||||
truth_odom_pub = nh.advertise<nav_msgs::Odometry>("true_odom", 10);
|
||||
feature_pub = nh.advertise<sensor_msgs::PointCloud2>(
|
||||
@ -412,6 +413,10 @@ void MsckfVio::imageCallback(
|
||||
return;
|
||||
}
|
||||
|
||||
old_imu_state = state_server.imu_state;
|
||||
old_true_state = true_state_server.imu_state;
|
||||
|
||||
|
||||
// Start the system if the first image is received.
|
||||
// The frame where the first image is received will be
|
||||
// the origin.
|
||||
@ -419,7 +424,6 @@ void MsckfVio::imageCallback(
|
||||
is_first_img = false;
|
||||
state_server.imu_state.time = feature_msg->header.stamp.toSec();
|
||||
}
|
||||
|
||||
static double max_processing_time = 0.0;
|
||||
static int critical_time_cntr = 0;
|
||||
double processing_start_time = ros::Time::now().toSec();
|
||||
@ -427,24 +431,14 @@ void MsckfVio::imageCallback(
|
||||
// Propogate the IMU state.
|
||||
// that are received before the image feature_msg.
|
||||
ros::Time start_time = ros::Time::now();
|
||||
|
||||
|
||||
batchImuProcessing(feature_msg->header.stamp.toSec());
|
||||
|
||||
if(GROUNDTRUTH)
|
||||
{
|
||||
state_server.imu_state.position = true_state_server.imu_state.position;
|
||||
state_server.imu_state.orientation = true_state_server.imu_state.orientation;
|
||||
state_server.imu_state.position_null = true_state_server.imu_state.position_null;
|
||||
state_server.imu_state.orientation_null = true_state_server.imu_state.orientation_null;
|
||||
|
||||
state_server.imu_state.R_imu_cam0 = true_state_server.imu_state.R_imu_cam0;
|
||||
state_server.imu_state.t_cam0_imu = true_state_server.imu_state.t_cam0_imu;
|
||||
}
|
||||
nh.param<int>("FILTER", FILTER, 0);
|
||||
|
||||
batchImuProcessing(feature_msg->header.stamp.toSec());
|
||||
|
||||
double imu_processing_time = (
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
cout << "1" << endl;
|
||||
// Augment the state vector.
|
||||
start_time = ros::Time::now();
|
||||
//truePhotometricStateAugmentation(feature_msg->header.stamp.toSec());
|
||||
@ -453,7 +447,7 @@ void MsckfVio::imageCallback(
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
|
||||
cout << "2" << endl;
|
||||
// cout << "2" << endl;
|
||||
// Add new observations for existing features or new
|
||||
// features in the map server.
|
||||
start_time = ros::Time::now();
|
||||
@ -462,7 +456,7 @@ void MsckfVio::imageCallback(
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
|
||||
cout << "3" << endl;
|
||||
// cout << "3" << endl;
|
||||
// Add new images to moving window
|
||||
start_time = ros::Time::now();
|
||||
manageMovingWindow(cam0_img, cam1_img, feature_msg);
|
||||
@ -470,14 +464,14 @@ void MsckfVio::imageCallback(
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
|
||||
cout << "4" << endl;
|
||||
// cout << "4" << endl;
|
||||
// Perform measurement update if necessary.
|
||||
start_time = ros::Time::now();
|
||||
removeLostFeatures();
|
||||
double remove_lost_features_time = (
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
cout << "5" << endl;
|
||||
// cout << "5" << endl;
|
||||
start_time = ros::Time::now();
|
||||
pruneLastCamStateBuffer();
|
||||
double prune_cam_states_time = (
|
||||
@ -487,7 +481,7 @@ void MsckfVio::imageCallback(
|
||||
batchTruthProcessing(feature_msg->header.stamp.toSec());
|
||||
|
||||
|
||||
cout << "6" << endl;
|
||||
// cout << "6" << endl;
|
||||
// Publish the odometry.
|
||||
start_time = ros::Time::now();
|
||||
publish(feature_msg->header.stamp);
|
||||
@ -504,18 +498,18 @@ void MsckfVio::imageCallback(
|
||||
++critical_time_cntr;
|
||||
ROS_INFO("\033[1;31mTotal processing time %f/%d...\033[0m",
|
||||
processing_time, critical_time_cntr);
|
||||
printf("IMU processing time: %f/%f\n",
|
||||
imu_processing_time, imu_processing_time/processing_time);
|
||||
printf("State augmentation time: %f/%f\n",
|
||||
state_augmentation_time, state_augmentation_time/processing_time);
|
||||
printf("Add observations time: %f/%f\n",
|
||||
add_observations_time, add_observations_time/processing_time);
|
||||
printf("Remove lost features time: %f/%f\n",
|
||||
remove_lost_features_time, remove_lost_features_time/processing_time);
|
||||
printf("Remove camera states time: %f/%f\n",
|
||||
prune_cam_states_time, prune_cam_states_time/processing_time);
|
||||
printf("Publish time: %f/%f\n",
|
||||
publish_time, publish_time/processing_time);
|
||||
//printf("IMU processing time: %f/%f\n",
|
||||
// imu_processing_time, imu_processing_time/processing_time);
|
||||
//printf("State augmentation time: %f/%f\n",
|
||||
// state_augmentation_time, state_augmentation_time/processing_time);
|
||||
//printf("Add observations time: %f/%f\n",
|
||||
// add_observations_time, add_observations_time/processing_time);
|
||||
//printf("Remove lost features time: %f/%f\n",
|
||||
// remove_lost_features_time, remove_lost_features_time/processing_time);
|
||||
//printf("Remove camera states time: %f/%f\n",
|
||||
// prune_cam_states_time, prune_cam_states_time/processing_time);
|
||||
//printf("Publish time: %f/%f\n",
|
||||
// publish_time, publish_time/processing_time);
|
||||
}
|
||||
|
||||
if(STREAMPAUSE)
|
||||
@ -806,8 +800,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
|
||||
// Counter how many IMU msgs in the buffer are used.
|
||||
int used_truth_msg_cntr = 0;
|
||||
|
||||
const IMUState old_true_state = true_state_server.imu_state;
|
||||
|
||||
for (const auto& truth_msg : truth_msg_buffer) {
|
||||
double truth_time = truth_msg.header.stamp.toSec();
|
||||
if (truth_time < true_state_server.imu_state.time) {
|
||||
@ -839,22 +831,76 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
|
||||
truth_msg_buffer.erase(truth_msg_buffer.begin(),
|
||||
truth_msg_buffer.begin()+used_truth_msg_cntr);
|
||||
|
||||
/*
|
||||
// calculate change
|
||||
delta_position = state_server.imu_state.position - old_imu_state.position;
|
||||
Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), old_imu_state.orientation);
|
||||
delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2);
|
||||
// calculate error in position
|
||||
|
||||
// calculate error in change
|
||||
|
||||
// calculate change
|
||||
Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - old_true_state.position;
|
||||
Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), old_true_state.orientation);
|
||||
Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2);
|
||||
Eigen::Vector3d error_delta_position = delta_true_position - delta_position;
|
||||
Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation;
|
||||
|
||||
double dx = (error_delta_position[0]/delta_true_position[0]);
|
||||
double dy = (error_delta_position[1]/delta_true_position[1]);
|
||||
double dz = (error_delta_position[2]/delta_true_position[2]);
|
||||
cout << "relative pos error: " << sqrt(dx*dx + dy*dy + dz*dz) * 100 << "%" << endl;
|
||||
|
||||
Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position;
|
||||
Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation);
|
||||
Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2);
|
||||
|
||||
double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
|
||||
sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
|
||||
|
||||
double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/
|
||||
sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2]));
|
||||
|
||||
double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
|
||||
sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
|
||||
|
||||
|
||||
cout << "relative pos error: " << relerr * 100 << "%" << endl;
|
||||
cout << "relative ori error: " << relOerr * 100 << "%" << endl;
|
||||
//cout << "absolute pos error: " <<
|
||||
*/
|
||||
if (eval_time + 1 < ros::Time::now().toSec())
|
||||
{
|
||||
|
||||
// calculate change
|
||||
delta_position = state_server.imu_state.position - timed_old_imu_state.position;
|
||||
Eigen::Vector4d delta_orientation_quaternion = quaternionMultiplication(quaternionConjugate(state_server.imu_state.orientation), timed_old_imu_state.orientation);
|
||||
delta_orientation = Eigen::Vector3d(delta_orientation_quaternion[0]*2, delta_orientation_quaternion[1]*2, delta_orientation_quaternion[2]*2);
|
||||
// calculate error in position
|
||||
|
||||
// calculate error in change
|
||||
|
||||
Eigen::Vector3d delta_true_position = true_state_server.imu_state.position - timed_old_true_state.position;
|
||||
Eigen::Vector4d delta_true_orientation = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), timed_old_true_state.orientation);
|
||||
Eigen::Vector3d delta_smallangle_true_orientation = Eigen::Vector3d(delta_true_orientation[0]*2, delta_true_orientation[1]*2, delta_true_orientation[2]*2);
|
||||
Eigen::Vector3d error_delta_position = delta_true_position - delta_position;
|
||||
Eigen::Vector3d error_delta_orientation = delta_smallangle_true_orientation - delta_orientation;
|
||||
|
||||
Eigen::Vector3d error_position = true_state_server.imu_state.position - state_server.imu_state.position;
|
||||
Eigen::Vector4d error_orientation_q = quaternionMultiplication(quaternionConjugate(true_state_server.imu_state.orientation), state_server.imu_state.orientation);
|
||||
Eigen::Vector3d error_orientation = Eigen::Vector3d(error_orientation_q[0]*2, error_orientation_q[1]*2, error_orientation_q[2]*2);
|
||||
|
||||
double relerr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
|
||||
sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
|
||||
|
||||
double relOerr = (sqrt(error_delta_orientation[0]*error_delta_orientation[0] + error_delta_orientation[1]*error_delta_orientation[1] + error_delta_orientation[2]*error_delta_orientation[2])/
|
||||
sqrt(delta_smallangle_true_orientation[0]*delta_smallangle_true_orientation[0] + delta_smallangle_true_orientation[1]*delta_smallangle_true_orientation[1] + delta_smallangle_true_orientation[2]*delta_smallangle_true_orientation[2]));
|
||||
|
||||
double abserr = (sqrt(error_delta_position[0]*error_delta_position[0] + error_delta_position[1]*error_delta_position[1] + error_delta_position[2]*error_delta_position[2])/
|
||||
sqrt(delta_true_position[0]*delta_true_position[0] + delta_true_position[1]*delta_true_position[1] + delta_true_position[2]*delta_true_position[2]));
|
||||
|
||||
// cout << "relative pos error: " << relerr * 100 << "%" << endl;
|
||||
// cout << "relative ori error: " << relOerr * 100 << "%" << endl;
|
||||
|
||||
timed_old_imu_state = state_server.imu_state;
|
||||
timed_old_true_state = true_state_server.imu_state;
|
||||
eval_time = ros::Time::now().toSec();
|
||||
}
|
||||
}
|
||||
|
||||
void MsckfVio::processTruthtoIMU(const double& time,
|
||||
@ -1477,17 +1523,60 @@ void MsckfVio::twodotFeatureJacobian(
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r)
|
||||
bool MsckfVio::PhotometricPatchPointResidual(
|
||||
const StateIDType& cam_state_id,
|
||||
const Feature& feature,
|
||||
VectorXd& r)
|
||||
{
|
||||
|
||||
// Prepare all the required data.
|
||||
VectorXd r_photo = VectorXd::Zero(N*N);
|
||||
int count = 0;
|
||||
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
|
||||
const CAMState& cam_state = state_server.cam_states[cam_state_id];
|
||||
const Feature& feature = map_server[feature_id];
|
||||
|
||||
//estimate photometric measurement
|
||||
std::vector<double> estimate_irradiance;
|
||||
std::vector<double> estimate_photo_z;
|
||||
std::vector<double> photo_z;
|
||||
|
||||
IlluminationParameter estimated_illumination;
|
||||
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
|
||||
for (auto& estimate_irradiance_j : estimate_irradiance)
|
||||
estimate_photo_z.push_back (estimate_irradiance_j *
|
||||
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
|
||||
estimated_illumination.frame_bias + estimated_illumination.feature_bias);
|
||||
|
||||
|
||||
for(auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
|
||||
|
||||
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
|
||||
// test if projection is inside frame
|
||||
if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1)
|
||||
return false;
|
||||
//add observation
|
||||
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
|
||||
|
||||
//calculate photom. residual
|
||||
r_photo(count) = photo_z[count] - estimate_photo_z[count];
|
||||
count++;
|
||||
}
|
||||
r = r_photo;
|
||||
return true;
|
||||
}
|
||||
|
||||
// generates the jacobian of one patch point regarding rho, anchor and current frame
|
||||
bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
const CAMState& cam_state,
|
||||
const Feature& feature,
|
||||
Eigen::Vector3d point,
|
||||
int count,
|
||||
Eigen::Matrix<double, 1, 1>& H_rhoj,
|
||||
Eigen::Matrix<double, 1, 6>& H_plj,
|
||||
Eigen::Matrix<double, 1, 6>& H_pAj)
|
||||
{
|
||||
|
||||
const StateIDType anchor_state_id = feature.observations.begin()->first;
|
||||
const CAMState anchor_state = state_server.cam_states[anchor_state_id];
|
||||
@ -1501,10 +1590,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
Matrix3d R_w_c1 = CAMState::T_cam0_cam1.linear() * R_w_c0;
|
||||
Vector3d t_c1_w = t_c0_w - R_w_c1.transpose()*CAMState::T_cam0_cam1.translation();
|
||||
|
||||
|
||||
//photometric observation
|
||||
std::vector<double> photo_z;
|
||||
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();
|
||||
@ -1517,6 +1603,69 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
Matrix<double, 3, 3> dCpij_dCGtheta = Matrix<double, 3, 3>::Zero();
|
||||
Matrix<double, 3, 3> dCpij_dGpC = Matrix<double, 3, 3>::Zero();
|
||||
|
||||
double dx, dy;
|
||||
|
||||
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
|
||||
cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point);
|
||||
|
||||
|
||||
// calculate derivation for anchor frame, use position for derivation calculation
|
||||
// frame derivative calculated convoluting with kernel [-1, 0, 1]
|
||||
dx = feature.cvKernel(p_in_anchor, "Sobel_x");
|
||||
dy = feature.cvKernel(p_in_anchor, "Sobel_y");
|
||||
|
||||
dI_dhj(0, 0) = dx * cam0.intrinsics[0];
|
||||
dI_dhj(0, 1) = dy * cam0.intrinsics[1];
|
||||
|
||||
//dh / d{}^Cp_{ij}
|
||||
dh_dCpij(0, 0) = 1 / p_c0(2);
|
||||
dh_dCpij(1, 1) = 1 / p_c0(2);
|
||||
dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
|
||||
dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
|
||||
|
||||
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
|
||||
|
||||
//orientation takes camera frame to world frame, we wa
|
||||
dh_dGpij = dh_dCpij * dCpij_dGpij;
|
||||
|
||||
//dh / d X_{pl}
|
||||
dCpij_dCGtheta = skewSymmetric(p_c0);
|
||||
dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
|
||||
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
|
||||
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
|
||||
|
||||
// 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));
|
||||
|
||||
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
|
||||
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
|
||||
feature.anchorPatch_ideal[count].y/(rho),
|
||||
1/(rho)));
|
||||
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
|
||||
|
||||
// Intermediate Jakobians
|
||||
H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
|
||||
H_plj = dI_dhj * dh_dXplj; // 1 x 6
|
||||
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r)
|
||||
{
|
||||
|
||||
// Prepare all the required data.
|
||||
const CAMState& cam_state = state_server.cam_states[cam_state_id];
|
||||
const Feature& feature = map_server[feature_id];
|
||||
|
||||
//photometric observation
|
||||
VectorXd r_photo = VectorXd::Zero(N*N);
|
||||
|
||||
// one line of the NxN Jacobians
|
||||
Eigen::Matrix<double, 1, 1> H_rhoj;
|
||||
Eigen::Matrix<double, 1, 6> H_plj;
|
||||
@ -1528,89 +1677,18 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
Eigen::MatrixXd H_pA(N*N, 6);
|
||||
|
||||
auto frame = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
auto anchor_frame = cam0.moving_window.find(anchor_state_id)->second.image;
|
||||
//observation
|
||||
const Vector4d& z = feature.observations.find(cam_state_id)->second;
|
||||
|
||||
//estimate photometric measurement
|
||||
std::vector<double> estimate_irradiance;
|
||||
std::vector<double> estimate_photo_z;
|
||||
IlluminationParameter estimated_illumination;
|
||||
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam0, estimate_irradiance, estimated_illumination);
|
||||
|
||||
// calculated here, because we need true 'estimate_irradiance' later for jacobi
|
||||
for (auto& estimate_irradiance_j : estimate_irradiance)
|
||||
estimate_photo_z.push_back (estimate_irradiance_j *
|
||||
estimated_illumination.frame_gain * estimated_illumination.feature_gain +
|
||||
estimated_illumination.frame_bias + estimated_illumination.feature_bias);
|
||||
// calcualte residual of patch
|
||||
PhotometricPatchPointResidual(cam_state_id, feature, r_photo);
|
||||
|
||||
// calculate jacobian for patch
|
||||
int count = 0;
|
||||
double dx, dy;
|
||||
|
||||
for (auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
//cout << "____feature-measurement_____\n" << endl;
|
||||
Eigen::Vector3d p_c0 = R_w_c0 * (point-t_c0_w);
|
||||
cv::Point2f p_in_c0 = feature.projectPositionToCamera(cam_state, cam_state_id, cam0, point);
|
||||
cv::Point2f p_in_anchor = feature.projectPositionToCamera(anchor_state, anchor_state_id, cam0, point);
|
||||
|
||||
if(p_in_c0.x < 0 or p_in_c0.x > frame.cols-1 or p_in_c0.y < 0 or p_in_c0.y > frame.rows-1)
|
||||
{
|
||||
cout << "skipped" << endl;
|
||||
return false;
|
||||
}
|
||||
//add observation
|
||||
photo_z.push_back(feature.PixelIrradiance(p_in_c0, frame));
|
||||
|
||||
//calculate photom. residual
|
||||
r_photo(count) = photo_z[count] - estimate_photo_z[count];
|
||||
//cout << "residual: " << photo_r.back() << endl;
|
||||
|
||||
// calculate derivation for anchor frame, use position for derivation calculation
|
||||
// frame derivative calculated convoluting with kernel [-1, 0, 1]
|
||||
|
||||
dx = feature.cvKernel(p_in_anchor, "Sobel_x");
|
||||
dy = feature.cvKernel(p_in_anchor, "Sobel_y");
|
||||
|
||||
// dx = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x+1, p_in_anchor.y), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x-1, p_in_anchor.y), anchor_frame);
|
||||
// dy = feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y+1), anchor_frame) - feature.PixelIrradiance(cv::Point2f(p_in_anchor.x, p_in_anchor.y-1), anchor_frame);
|
||||
|
||||
dI_dhj(0, 0) = dx * cam0.intrinsics[0];
|
||||
dI_dhj(0, 1) = dy * cam0.intrinsics[1];
|
||||
|
||||
//dh / d{}^Cp_{ij}
|
||||
dh_dCpij(0, 0) = 1 / p_c0(2);
|
||||
dh_dCpij(1, 1) = 1 / p_c0(2);
|
||||
dh_dCpij(0, 2) = -(p_c0(0))/(p_c0(2)*p_c0(2));
|
||||
dh_dCpij(1, 2) = -(p_c0(1))/(p_c0(2)*p_c0(2));
|
||||
|
||||
dCpij_dGpij = quaternionToRotation(cam_state.orientation);
|
||||
|
||||
//orientation takes camera frame to world frame, we wa
|
||||
dh_dGpij = dh_dCpij * dCpij_dGpij;
|
||||
|
||||
//dh / d X_{pl}
|
||||
dCpij_dCGtheta = skewSymmetric(p_c0);
|
||||
dCpij_dGpC = -quaternionToRotation(cam_state.orientation);
|
||||
dh_dXplj.block<2, 3>(0, 0) = dh_dCpij * dCpij_dCGtheta;
|
||||
dh_dXplj.block<2, 3>(0, 3) = dh_dCpij * dCpij_dGpC;
|
||||
|
||||
// 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));
|
||||
|
||||
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
|
||||
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[count].x/(rho),
|
||||
feature.anchorPatch_ideal[count].y/(rho),
|
||||
1/(rho)));
|
||||
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
|
||||
|
||||
// Intermediate Jakobians
|
||||
H_rhoj = dI_dhj * dh_dGpij * dGpj_drhoj; // 1 x 1
|
||||
H_plj = dI_dhj * dh_dXplj; // 1 x 6
|
||||
H_pAj = dI_dhj * dh_dGpij * dGpj_XpAj; // 1 x 6
|
||||
|
||||
// get jacobi of single point in patch
|
||||
PhotometricPatchPointJacobian(cam_state, feature, point, count, H_rhoj, H_plj, H_pAj);
|
||||
|
||||
// stack point into entire jacobi
|
||||
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;
|
||||
@ -1618,48 +1696,31 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
count++;
|
||||
}
|
||||
|
||||
cout << "done" << endl;
|
||||
// construct the jacobian structure needed for nullspacing
|
||||
MatrixXd H_xl = MatrixXd::Zero(N*N, 21+state_server.cam_states.size()*7);
|
||||
MatrixXd H_yl = MatrixXd::Zero(N*N, N*N+1);
|
||||
|
||||
// set anchor Jakobi
|
||||
// get position of anchor in cam states
|
||||
ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
|
||||
|
||||
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
|
||||
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
|
||||
H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA;
|
||||
|
||||
// set frame Jakobi
|
||||
//get position of current frame in cam states
|
||||
auto cam_state_iter = state_server.cam_states.find(cam_state_id);
|
||||
int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
|
||||
|
||||
// set jakobi of state
|
||||
H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl;
|
||||
|
||||
// set ones for irradiance bias
|
||||
// H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
|
||||
|
||||
// set irradiance error Block
|
||||
H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N);
|
||||
|
||||
// TODO make this calculation more fluent
|
||||
|
||||
/*for(int i = 0; i< N*N; i++)
|
||||
H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
|
||||
*/
|
||||
|
||||
H_yl.block(0, N*N, N*N, 1) = -H_rho;
|
||||
|
||||
// set to return values
|
||||
H_x = H_xl;
|
||||
H_y = H_yl;
|
||||
|
||||
r = r_photo;
|
||||
|
||||
std::stringstream ss;
|
||||
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
{
|
||||
// pregenerating information for visualization
|
||||
std::stringstream ss;
|
||||
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
|
||||
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
|
||||
//get position of current frame in cam states
|
||||
auto cam_state_iter = state_server.cam_states.find(cam_state_id);
|
||||
int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
|
||||
|
||||
ss << "INFO:" << " anchor: " << cam_state_cntr_anchor << " frame: " << cam_state_cntr;
|
||||
|
||||
// visualizing functions
|
||||
feature.MarkerGeneration(marker_pub, state_server.cam_states);
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
|
||||
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
|
||||
@ -1668,6 +1729,43 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
return true;
|
||||
}
|
||||
|
||||
bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
|
||||
Eigen::MatrixXd& H_pl,
|
||||
Eigen::MatrixXd& H_pA,
|
||||
const Feature& feature,
|
||||
const StateIDType& cam_state_id,
|
||||
Eigen::MatrixXd& H_xl,
|
||||
Eigen::MatrixXd& H_yl)
|
||||
{
|
||||
// get position of anchor in cam states
|
||||
auto cam_state_anchor = state_server.cam_states.find(feature.observations.begin()->first);
|
||||
int cam_state_cntr_anchor = std::distance(state_server.cam_states.begin(), cam_state_anchor);
|
||||
// set anchor Jakobi
|
||||
H_xl.block(0, 21+cam_state_cntr_anchor*7, N*N, 6) = -H_pA;
|
||||
|
||||
//get position of current frame in cam states
|
||||
auto cam_state_iter = state_server.cam_states.find(cam_state_id);
|
||||
// set frame Jakobi
|
||||
int cam_state_cntr = std::distance(state_server.cam_states.begin(), cam_state_iter);
|
||||
|
||||
// set jakobi of state
|
||||
H_xl.block(0, 21+cam_state_cntr*7, N*N, 6) = -H_pl;
|
||||
|
||||
// set ones for irradiance bias
|
||||
// H_xl.block(0, 21+cam_state_cntr*7+6, N*N, 1) = Eigen::ArrayXd::Ones(N*N);
|
||||
|
||||
// set irradiance error Block
|
||||
H_yl.block(0, 0,N*N, N*N) = /* estimated_illumination.feature_gain * estimated_illumination.frame_gain * */ Eigen::MatrixXd::Identity(N*N, N*N);
|
||||
|
||||
|
||||
/*for(int i = 0; i< N*N; i++)
|
||||
H_yl(i, N*N+cam_state_cntr) = estimate_irradiance[i];
|
||||
*/
|
||||
|
||||
H_yl.block(0, N*N, N*N, 1) = -H_rho;
|
||||
|
||||
}
|
||||
|
||||
|
||||
bool MsckfVio::PhotometricFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
@ -1717,11 +1815,9 @@ bool MsckfVio::PhotometricFeatureJacobian(
|
||||
r_i.segment(stack_cntr, N*N) = r_l;
|
||||
stack_cntr += N*N;
|
||||
}
|
||||
if(stack_cntr == 0)
|
||||
{
|
||||
cout << "skip feature" << endl;
|
||||
if(stack_cntr < 2*N*N)
|
||||
return false;
|
||||
}
|
||||
|
||||
// Project the residual and Jacobians onto the nullspace
|
||||
// of H_yj.
|
||||
|
||||
@ -2027,8 +2123,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
VectorXd delta_x = K * r;
|
||||
|
||||
|
||||
cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
|
||||
cout << "reg update: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
|
||||
// cout << "reg rotate: " << delta_x[0] << ", " << delta_x[1] << ", " << delta_x[2] << endl;
|
||||
cout << "reg: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
|
||||
|
||||
if(FILTER != 0) return;
|
||||
|
||||
@ -2053,9 +2149,6 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
|
||||
myfile.close();
|
||||
}
|
||||
delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
|
||||
delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
|
||||
|
||||
// Update the IMU state.
|
||||
|
||||
const VectorXd& delta_x_imu = delta_x.head<21>();
|
||||
@ -2239,10 +2332,13 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
|
||||
// Perform QR decompostion on H_sparse.
|
||||
SPQR<SparseMatrix<double> > spqr_helper;
|
||||
spqr_helper.setSPQROrdering(SPQR_ORDERING_NATURAL);
|
||||
spqr_helper.compute(H_sparse);
|
||||
|
||||
cout << "comp" << endl;
|
||||
spqr_helper.compute(H_sparse);
|
||||
cout << "done" << endl;
|
||||
MatrixXd H_temp;
|
||||
VectorXd r_temp;
|
||||
|
||||
(spqr_helper.matrixQ().transpose() * H).evalTo(H_temp);
|
||||
(spqr_helper.matrixQ().transpose() * r).evalTo(r_temp);
|
||||
|
||||
@ -2261,6 +2357,7 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
|
||||
Feature::observation_noise*MatrixXd::Identity(
|
||||
H_thin.rows(), H_thin.rows());
|
||||
//MatrixXd K_transpose = S.fullPivHouseholderQr().solve(H*P);
|
||||
|
||||
MatrixXd K_transpose = S.ldlt().solve(H_thin*P);
|
||||
MatrixXd K = K_transpose.transpose();
|
||||
// Compute the error of the state.
|
||||
@ -2271,28 +2368,27 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
|
||||
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
//octave
|
||||
ofstream myfile;
|
||||
|
||||
myfile.open("/home/raphael/dev/octave/measurement2octave");
|
||||
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
|
||||
<< "# name: K\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << K.rows() << "\n"
|
||||
<< "# columns: " << K.cols() << "\n"
|
||||
<< K << endl;
|
||||
//octave
|
||||
ofstream myfile;
|
||||
|
||||
myfile.open("/home/raphael/dev/octave/measurement2octave");
|
||||
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
|
||||
<< "# name: K\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << K.rows() << "\n"
|
||||
<< "# columns: " << K.cols() << "\n"
|
||||
<< K << endl;
|
||||
|
||||
myfile << "# name: r\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << r.rows() << "\n"
|
||||
<< "# columns: " << r.cols() << "\n"
|
||||
<< r << endl;
|
||||
myfile << "# name: r\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << r.rows() << "\n"
|
||||
<< "# columns: " << r.cols() << "\n"
|
||||
<< r << endl;
|
||||
|
||||
myfile.close();
|
||||
myfile.close();
|
||||
}
|
||||
|
||||
delta_position = Eigen::Vector3d(delta_x[12], delta_x[13], delta_x[14]);
|
||||
delta_orientation = Eigen::Vector3d(delta_x[0], delta_x[1], delta_x[2]);
|
||||
cout << "pho: " << delta_x[12] << ", " << delta_x[13] << ", " << delta_x[14] << endl;
|
||||
|
||||
const VectorXd& delta_x_imu = delta_x.head<21>();
|
||||
|
||||
@ -2349,8 +2445,8 @@ void MsckfVio::photometricMeasurementUpdate(const MatrixXd& H, const VectorXd& r
|
||||
|
||||
|
||||
bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof) {
|
||||
MatrixXd P1 = H * state_server.state_cov * H.transpose();
|
||||
|
||||
MatrixXd P1 = H * state_server.state_cov * H.transpose();
|
||||
|
||||
MatrixXd P2 = Feature::observation_noise *
|
||||
MatrixXd::Identity(H.rows(), H.rows());
|
||||
@ -2363,10 +2459,10 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof)
|
||||
if (chi_squared_test_table[dof] == 0)
|
||||
return false;
|
||||
if (gamma < chi_squared_test_table[dof]) {
|
||||
//cout << "passed" << endl;
|
||||
// cout << "passed" << endl;
|
||||
return true;
|
||||
} else {
|
||||
//cout << "failed" << endl;
|
||||
// cout << "failed" << endl;
|
||||
return false;
|
||||
}
|
||||
}
|
||||
@ -2465,16 +2561,15 @@ void MsckfVio::removeLostFeatures() {
|
||||
VectorXd pr_j;
|
||||
MatrixXd twoH_xj;
|
||||
VectorXd twor_j;
|
||||
|
||||
/*
|
||||
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true);
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j) == true)
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
|
||||
@ -2499,10 +2594,11 @@ void MsckfVio::removeLostFeatures() {
|
||||
{
|
||||
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
|
||||
pr.conservativeResize(pstack_cntr);
|
||||
|
||||
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
}
|
||||
|
||||
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
|
||||
@ -2648,17 +2744,15 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
for (const auto& cam_state : state_server.cam_states)
|
||||
involved_cam_state_ids.push_back(cam_state.first);
|
||||
|
||||
/*
|
||||
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true);
|
||||
{
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
}
|
||||
}*/
|
||||
|
||||
}
|
||||
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
@ -2679,26 +2773,26 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
feature.observations.erase(cam_id);
|
||||
}
|
||||
|
||||
if (pstack_cntr)
|
||||
|
||||
if(pstack_cntr)
|
||||
{
|
||||
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
|
||||
pr.conservativeResize(pstack_cntr);
|
||||
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
}
|
||||
|
||||
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
// Perform measurement update.
|
||||
|
||||
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
// Perform measurement update.
|
||||
|
||||
|
||||
|
||||
// Perform the measurement update step.
|
||||
measurementUpdate(H_x, r);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
|
||||
//reduction
|
||||
int cam_sequence = std::distance(state_server.cam_states.begin(),
|
||||
state_server.cam_states.find(rm_cam_state_id));
|
||||
@ -2831,7 +2925,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
if (involved_cam_state_ids.size() == 0) continue;
|
||||
|
||||
/*
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j) == true)
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size())) {// involved_cam_state_ids.size())) {
|
||||
@ -2839,7 +2933,8 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
pr.segment(pstack_cntr, pr_j.rows()) = pr_j;
|
||||
pstack_cntr += pH_xj.rows();
|
||||
}
|
||||
}*/
|
||||
}
|
||||
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
|
||||
@ -2860,23 +2955,24 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
}
|
||||
|
||||
|
||||
if(pstack_cntr > 0)
|
||||
|
||||
if(pstack_cntr)
|
||||
{
|
||||
pH_x.conservativeResize(pstack_cntr, pH_x.cols());
|
||||
pr.conservativeResize(pstack_cntr);
|
||||
|
||||
pr.conservativeResize(pstack_cntr);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
}
|
||||
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
|
||||
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
|
||||
// Perform measurement update.
|
||||
|
||||
// Perform the measurement update step.
|
||||
measurementUpdate(H_x, r);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
|
||||
//reduction
|
||||
for (const auto& cam_id : rm_cam_state_ids) {
|
||||
int cam_sequence = std::distance(state_server.cam_states.begin(),
|
||||
|
||||
Loading…
Reference in New Issue
Block a user