Compare commits
6 Commits
photometry
...
photometry
| Author | SHA1 | Date | |
|---|---|---|---|
| 5b87d5b3bb | |||
| 5ba25f8f95 | |||
| 3b6d2c918d | |||
| 55669ea2c9 | |||
| 4a604e6dca | |||
| 30daf37202 |
@ -9,7 +9,7 @@ cam0:
|
||||
0, 0, 0, 1.000000000000000]
|
||||
camera_model: pinhole
|
||||
distortion_coeffs: [-0.28340811, 0.07395907, 0.00019359, 1.76187114e-05]
|
||||
distortion_model: pre-radtan
|
||||
distortion_model: radtan
|
||||
intrinsics: [458.654, 457.296, 367.215, 248.375]
|
||||
resolution: [752, 480]
|
||||
timeshift_cam_imu: 0.0
|
||||
@ -26,7 +26,7 @@ cam1:
|
||||
0, 0, 0, 1.000000000000000]
|
||||
camera_model: pinhole
|
||||
distortion_coeffs: [-0.28368365, 0.07451284, -0.00010473, -3.55590700e-05]
|
||||
distortion_model: pre-radtan
|
||||
distortion_model: radtan
|
||||
intrinsics: [457.587, 456.134, 379.999, 255.238]
|
||||
resolution: [752, 480]
|
||||
timeshift_cam_imu: 0.0
|
||||
|
||||
@ -7,7 +7,7 @@ cam0:
|
||||
camera_model: pinhole
|
||||
distortion_coeffs: [0.010171079892421483, -0.010816440029919381, 0.005942781769412756,
|
||||
-0.001662284667857643]
|
||||
distortion_model: equidistant
|
||||
distortion_model: pre-equidistant
|
||||
intrinsics: [380.81042871360756, 380.81194179427075, 510.29465304840727, 514.3304630538506]
|
||||
resolution: [1024, 1024]
|
||||
rostopic: /cam0/image_raw
|
||||
@ -25,7 +25,7 @@ cam1:
|
||||
camera_model: pinhole
|
||||
distortion_coeffs: [0.01371679169245271, -0.015567360615942622, 0.00905043103315326,
|
||||
-0.002347858896562788]
|
||||
distortion_model: equidistant
|
||||
distortion_model: pre-equidistant
|
||||
intrinsics: [379.2869884263036, 379.26583742214524, 505.5666703237407, 510.2840961765407]
|
||||
resolution: [1024, 1024]
|
||||
rostopic: /cam1/image_raw
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
*/
|
||||
|
||||
#ifndef MSCKF_VIO_FEATURE_H
|
||||
#define MSCKF_VIO_FEATURE_H
|
||||
#define MSCKF_VIO_FEATURE_Hs
|
||||
|
||||
#include <iostream>
|
||||
#include <map>
|
||||
@ -137,7 +137,6 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
|
||||
inline bool checkMotion(
|
||||
const CamStateServer& cam_states) const;
|
||||
|
||||
|
||||
/*
|
||||
* @brief InitializeAnchor generates the NxN patch around the
|
||||
* feature in the Anchor image
|
||||
@ -172,6 +171,7 @@ void Rhocost(const Eigen::Isometry3d& T_c0_ci,
|
||||
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
|
||||
@ -224,7 +224,7 @@ bool VisualizeKernel(
|
||||
bool VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam,
|
||||
CameraCalibration& cam0,
|
||||
const Eigen::VectorXd& photo_r,
|
||||
std::stringstream& ss) const;
|
||||
/*
|
||||
@ -713,22 +713,26 @@ bool Feature::VisualizeKernel(
|
||||
//cv::Sobel(anchorImage, yderImage, CV_8UC1, 0, 1, 3);
|
||||
|
||||
|
||||
// cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
|
||||
// cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
|
||||
cv::Mat xderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
|
||||
cv::Mat yderImage2(anchorImage.rows, anchorImage.cols, anchorImage_blurred.type());
|
||||
|
||||
|
||||
cv::Mat norm_abs_xderImage;
|
||||
cv::normalize(abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
|
||||
|
||||
cv::imshow("xder", norm_abs_xderImage);
|
||||
cv::imshow("yder", abs_yderImage);
|
||||
|
||||
/*
|
||||
for(int i = 1; i < anchorImage.rows-1; i++)
|
||||
for(int j = 1; j < anchorImage.cols-1; j++)
|
||||
xderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_x"));
|
||||
|
||||
|
||||
for(int i = 1; i < anchorImage.rows-1; i++)
|
||||
for(int j = 1; j < anchorImage.cols-1; j++)
|
||||
yderImage2.at<uint8_t>(j,i) = 255.*fabs(Kernel(cv::Point2f(i,j), anchorImage_blurred, "Sobel_y"));
|
||||
*/
|
||||
//cv::imshow("anchor", anchorImage);
|
||||
cv::imshow("xder2", xderImage);
|
||||
cv::imshow("yder2", yderImage);
|
||||
cv::imshow("anchor", anchorImage);
|
||||
cv::imshow("xder2", xderImage2);
|
||||
cv::imshow("yder2", yderImage2);
|
||||
|
||||
cvWaitKey(0);
|
||||
}
|
||||
@ -736,7 +740,7 @@ bool Feature::VisualizeKernel(
|
||||
bool Feature::VisualizePatch(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
CameraCalibration& cam,
|
||||
CameraCalibration& cam0,
|
||||
const Eigen::VectorXd& photo_r,
|
||||
std::stringstream& ss) const
|
||||
{
|
||||
@ -745,7 +749,7 @@ bool Feature::VisualizePatch(
|
||||
|
||||
//visu - anchor
|
||||
auto anchor = observations.begin();
|
||||
cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
|
||||
cv::Mat anchorImage = cam0.moving_window.find(anchor->first)->second.image;
|
||||
cv::Mat dottedFrame(anchorImage.size(), CV_8UC3);
|
||||
cv::cvtColor(anchorImage, dottedFrame, CV_GRAY2RGB);
|
||||
|
||||
@ -757,10 +761,10 @@ bool Feature::VisualizePatch(
|
||||
cv::Point ys(point.x, point.y);
|
||||
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,255));
|
||||
}
|
||||
cam.featureVisu = dottedFrame.clone();
|
||||
cam0.featureVisu = dottedFrame.clone();
|
||||
|
||||
// visu - feature
|
||||
cv::Mat current_image = cam.moving_window.find(cam_state_id)->second.image;
|
||||
cv::Mat current_image = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
cv::cvtColor(current_image, dottedFrame, CV_GRAY2RGB);
|
||||
|
||||
// set position in frame
|
||||
@ -768,7 +772,7 @@ bool Feature::VisualizePatch(
|
||||
std::vector<double> projectionPatch;
|
||||
for(auto point : anchorPatch_3d)
|
||||
{
|
||||
cv::Point2f p_in_c0 = projectPositionToCamera(cam_state, cam_state_id, cam, point);
|
||||
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);
|
||||
@ -776,7 +780,7 @@ bool Feature::VisualizePatch(
|
||||
cv::rectangle(dottedFrame, xs, ys, cv::Scalar(0,255,0));
|
||||
}
|
||||
|
||||
cv::hconcat(cam.featureVisu, dottedFrame, cam.featureVisu);
|
||||
cv::hconcat(cam0.featureVisu, dottedFrame, cam0.featureVisu);
|
||||
|
||||
|
||||
// patches visualization
|
||||
@ -823,14 +827,10 @@ bool Feature::VisualizePatch(
|
||||
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;
|
||||
if(cam.id == 0)
|
||||
p_f = cv::Point2f(observations.find(cam_state_id)->second(0),observations.find(cam_state_id)->second(1));
|
||||
else if(cam.id == 1)
|
||||
p_f = cv::Point2f(observations.find(cam_state_id)->second(2),observations.find(cam_state_id)->second(3));
|
||||
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);
|
||||
|
||||
p_f = image_handler::distortPoint(p_f, cam.intrinsics, cam.distortion_model, cam.distortion_coeffs);
|
||||
for(int i = 0; i<N; i++)
|
||||
{
|
||||
for(int j = 0; j<N ; j++)
|
||||
@ -853,17 +853,17 @@ bool Feature::VisualizePatch(
|
||||
|
||||
for(int i = 0; i<N; i++)
|
||||
for(int j = 0; j<N; j++)
|
||||
if(photo_r(2*(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(2*(i*N+j)+1)*255, 255 - photo_r(2*(i*N+j)+1)*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(2*(i*N+j)+1)*255, 255 + photo_r(2*(i*N+j)+1)*255),
|
||||
cv::Scalar(255, 255 + photo_r(i*N+j)*255, 255 + photo_r(i*N+j)*255),
|
||||
CV_FILLED);
|
||||
|
||||
// gradient arrow
|
||||
@ -884,7 +884,7 @@ bool Feature::VisualizePatch(
|
||||
3);
|
||||
*/
|
||||
|
||||
cv::hconcat(cam.featureVisu, irradianceFrame, cam.featureVisu);
|
||||
cv::hconcat(cam0.featureVisu, irradianceFrame, cam0.featureVisu);
|
||||
|
||||
/*
|
||||
// visualize position of used observations and resulting feature position
|
||||
@ -916,15 +916,15 @@ bool Feature::VisualizePatch(
|
||||
|
||||
// draw, x y position and arrow with direction - write z next to it
|
||||
|
||||
cv::resize(cam.featureVisu, cam.featureVisu, cv::Size(), rescale, rescale);
|
||||
cv::resize(cam0.featureVisu, cam0.featureVisu, cv::Size(), rescale, rescale);
|
||||
|
||||
|
||||
cv::hconcat(cam.featureVisu, positionFrame, cam.featureVisu);
|
||||
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(cam.featureVisu, ss.str() , cvPoint(anchorImage.rows + 100, anchorImage.cols - 30),
|
||||
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?
|
||||
|
||||
@ -932,10 +932,10 @@ bool Feature::VisualizePatch(
|
||||
|
||||
std::stringstream loc;
|
||||
// loc << "/home/raphael/dev/MSCKF_ws/img/feature_" << std::to_string(ros::Time::now().toSec()) << ".jpg";
|
||||
//cv::imwrite(loc.str(), cam.featureVisu);
|
||||
//cv::imwrite(loc.str(), cam0.featureVisu);
|
||||
|
||||
cv::imshow("patch", cam.featureVisu);
|
||||
cvWaitKey(1);
|
||||
cv::imshow("patch", cam0.featureVisu);
|
||||
cvWaitKey(0);
|
||||
}
|
||||
|
||||
float Feature::PixelIrradiance(cv::Point2f pose, cv::Mat image) const
|
||||
@ -979,6 +979,8 @@ cv::Point2f Feature::pixelDistanceAt(
|
||||
return distance;
|
||||
}
|
||||
|
||||
|
||||
|
||||
cv::Point2f Feature::projectPositionToCamera(
|
||||
const CAMState& cam_state,
|
||||
const StateIDType& cam_state_id,
|
||||
@ -995,6 +997,7 @@ cv::Point2f Feature::projectPositionToCamera(
|
||||
Eigen::Matrix3d R_w_c0 = quaternionToRotation(cam_state.orientation);
|
||||
const Eigen::Vector3d& t_c0_w = cam_state.position;
|
||||
|
||||
|
||||
// project point according to model
|
||||
if(cam.id == 0)
|
||||
{
|
||||
@ -1046,9 +1049,8 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
|
||||
|
||||
auto anchor = observations.begin();
|
||||
if(cam.moving_window.find(anchor->first) == cam.moving_window.end())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
cv::Mat anchorImage = cam.moving_window.find(anchor->first)->second.image;
|
||||
cv::Mat anchorImage_deeper;
|
||||
anchorImage.convertTo(anchorImage_deeper,CV_16S);
|
||||
@ -1071,16 +1073,17 @@ bool Feature::initializeAnchor(const CameraCalibration& cam, int N)
|
||||
|
||||
|
||||
// check if image has been pre-undistorted
|
||||
if(cam.distortion_model.substr(0,3) == "pre")
|
||||
if(cam.distortion_model.substr(0,3) == "pre-")
|
||||
{
|
||||
std::cout << "is a pre" << std::endl;
|
||||
//project onto pixel plane
|
||||
undist_anchor_center_pos = cv::Point2f(u * cam.intrinsics[0] + cam.intrinsics[2], v * cam.intrinsics[1] + cam.intrinsics[3]);
|
||||
|
||||
// create vector of patch in pixel plane
|
||||
for(double u_run = -n; u_run <= n; u_run++)
|
||||
for(double v_run = -n; v_run <= n; v_run++)
|
||||
anchorPatch_real.push_back(cv::Point2f(undist_anchor_center_pos.x+u_run, undist_anchor_center_pos.y+v_run));
|
||||
|
||||
|
||||
//project back into u,v
|
||||
for(int i = 0; i < N*N; i++)
|
||||
anchorPatch_ideal.push_back(cv::Point2f((anchorPatch_real[i].x-cam.intrinsics[2])/cam.intrinsics[0], (anchorPatch_real[i].y-cam.intrinsics[3])/cam.intrinsics[1]));
|
||||
|
||||
@ -320,8 +320,6 @@ private:
|
||||
return;
|
||||
}
|
||||
|
||||
bool STREAMPAUSE;
|
||||
|
||||
// Indicate if this is the first image message.
|
||||
bool is_first_img;
|
||||
|
||||
|
||||
@ -202,7 +202,7 @@ class MsckfVio {
|
||||
Eigen::Vector4d& r);
|
||||
// This function computes the Jacobian of all measurements viewed
|
||||
// in the given camera states of this feature.
|
||||
bool featureJacobian(
|
||||
void featureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
@ -220,12 +220,14 @@ class MsckfVio {
|
||||
const Feature& feature,
|
||||
const StateIDType& cam_state_id,
|
||||
Eigen::MatrixXd& H_xl,
|
||||
Eigen::MatrixXd& H_yl);
|
||||
Eigen::MatrixXd& H_yl,
|
||||
int patchsize);
|
||||
|
||||
bool PhotometricPatchPointResidual(
|
||||
const StateIDType& cam_state_id,
|
||||
const Feature& feature,
|
||||
Eigen::VectorXd& r);
|
||||
Eigen::VectorXd& r,
|
||||
int patchsize);
|
||||
|
||||
bool PhotometricPatchPointJacobian(
|
||||
const CAMState& cam_state,
|
||||
@ -233,6 +235,7 @@ class MsckfVio {
|
||||
const Feature& feature,
|
||||
Eigen::Vector3d point,
|
||||
int count,
|
||||
int patchsize,
|
||||
Eigen::Matrix<double, 2, 1>& H_rhoj,
|
||||
Eigen::Matrix<double, 2, 6>& H_plj,
|
||||
Eigen::Matrix<double, 2, 6>& H_pAj,
|
||||
@ -243,10 +246,10 @@ class MsckfVio {
|
||||
const FeatureIDType& feature_id,
|
||||
Eigen::MatrixXd& H_x,
|
||||
Eigen::MatrixXd& H_y,
|
||||
Eigen::VectorXd& r);
|
||||
Eigen::VectorXd& r,
|
||||
int patchsize);
|
||||
|
||||
|
||||
bool twodotFeatureJacobian(
|
||||
void twodotFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
@ -254,7 +257,8 @@ class MsckfVio {
|
||||
bool PhotometricFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r);
|
||||
Eigen::MatrixXd& H_x, Eigen::VectorXd& r,
|
||||
int patchsize);
|
||||
|
||||
void photometricMeasurementUpdate(const Eigen::MatrixXd& H, const Eigen::VectorXd& r);
|
||||
void measurementUpdate(const Eigen::MatrixXd& H,
|
||||
@ -283,12 +287,10 @@ class MsckfVio {
|
||||
bool nan_flag;
|
||||
bool play;
|
||||
double last_time_bound;
|
||||
double time_offset;
|
||||
|
||||
// Patch size for Photometry
|
||||
int N;
|
||||
// Image rescale
|
||||
int SCALE;
|
||||
|
||||
// Chi squared test table.
|
||||
static std::map<int, double> chi_squared_test_table;
|
||||
|
||||
|
||||
@ -11,14 +11,11 @@
|
||||
output="screen"
|
||||
>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="StreamPause" value="true"/>
|
||||
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
<param name="grid_row" value="4"/>
|
||||
<param name="grid_col" value="4"/>
|
||||
<param name="grid_min_feature_num" value="3"/>
|
||||
<param name="grid_max_feature_num" value="5"/>
|
||||
<param name="grid_max_feature_num" value="4"/>
|
||||
<param name="pyramid_levels" value="3"/>
|
||||
<param name="patch_size" value="15"/>
|
||||
<param name="fast_threshold" value="10"/>
|
||||
@ -27,10 +24,9 @@
|
||||
<param name="ransac_threshold" value="3"/>
|
||||
<param name="stereo_threshold" value="5"/>
|
||||
|
||||
<remap from="~imu" to="/imu0"/>
|
||||
<remap from="~cam0_image" to="/cam0/image_raw"/>
|
||||
<remap from="~cam1_image" to="/cam1/image_raw"/>
|
||||
|
||||
<remap from="~imu" to="/imu02"/>
|
||||
<remap from="~cam0_image" to="/cam0/image_raw2"/>
|
||||
<remap from="~cam1_image" to="/cam1/image_raw2"/>
|
||||
|
||||
</node>
|
||||
</group>
|
||||
|
||||
@ -11,9 +11,6 @@
|
||||
output="screen"
|
||||
>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="StreamPause" value="true"/>
|
||||
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
<param name="grid_row" value="4"/>
|
||||
<param name="grid_col" value="4"/>
|
||||
|
||||
@ -17,17 +17,15 @@
|
||||
args='standalone msckf_vio/MsckfVioNodelet'
|
||||
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="true"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="5"/>
|
||||
<param name="image_scale" value ="1"/>
|
||||
<param name="patch_size_n" value="3"/>
|
||||
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
@ -17,16 +17,15 @@
|
||||
args='standalone msckf_vio/MsckfVioNodelet'
|
||||
output="screen">
|
||||
|
||||
<param name="FILTER" value="0"/>
|
||||
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
|
||||
<param name="FILTER" value="1"/>
|
||||
|
||||
<!-- Debugging Flaggs -->
|
||||
<param name="StreamPause" value="true"/>
|
||||
<param name="PrintImages" value="false"/>
|
||||
<param name="PrintImages" value="true"/>
|
||||
<param name="GroundTruth" value="false"/>
|
||||
|
||||
<param name="patch_size_n" value="3"/>
|
||||
<param name="image_scale" value ="1"/>
|
||||
|
||||
<param name="patch_size_n" value="5"/>
|
||||
<!-- Calibration parameters -->
|
||||
<rosparam command="load" file="$(arg calibration_file)"/>
|
||||
|
||||
@ -34,7 +33,7 @@
|
||||
<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="12"/>
|
||||
<param name="max_cam_state_size" value="20"/>
|
||||
<param name="position_std_threshold" value="8.0"/>
|
||||
|
||||
<param name="rotation_threshold" value="0.2618"/>
|
||||
|
||||
@ -17,7 +17,6 @@
|
||||
args='standalone msckf_vio/MsckfVioNodelet'
|
||||
output="screen">
|
||||
|
||||
|
||||
<!-- Filter Flag, 0 = msckf, 1 = photometric, 2 = two -->
|
||||
<param name="FILTER" value="1"/>
|
||||
|
||||
@ -34,7 +33,7 @@
|
||||
<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="max_cam_state_size" value="12"/>
|
||||
<param name="position_std_threshold" value="8.0"/>
|
||||
|
||||
<param name="rotation_threshold" value="0.2618"/>
|
||||
|
||||
@ -1,64 +0,0 @@
|
||||
#!/usr/bin/env python
|
||||
import rosbag
|
||||
import rospy
|
||||
from sensor_msgs.msg import Imu, Image
|
||||
from geometry_msgs.msg import TransformStamped
|
||||
import time
|
||||
import signal
|
||||
import sys
|
||||
|
||||
|
||||
def signal_handler(sig, frame):
|
||||
print('gracefully exiting the program.')
|
||||
bag.close()
|
||||
sys.exit(0)
|
||||
|
||||
def main():
|
||||
global bag
|
||||
|
||||
cam0_topic = '/cam0/image_raw'
|
||||
cam1_topic = '/cam1/image_raw'
|
||||
imu0_topic = '/imu0'
|
||||
grnd_topic = '/vrpn_client/raw_transform'
|
||||
|
||||
rospy.init_node('controlbag')
|
||||
rospy.set_param('play_bag', False)
|
||||
|
||||
cam0_pub = rospy.Publisher(cam0_topic, Image, queue_size=10)
|
||||
cam1_pub = rospy.Publisher(cam1_topic, Image, queue_size=10)
|
||||
imu0_pub = rospy.Publisher(imu0_topic, Imu, queue_size=10)
|
||||
grnd_pub = rospy.Publisher(grnd_topic, TransformStamped, queue_size=10)
|
||||
|
||||
signal.signal(signal.SIGINT, signal_handler)
|
||||
|
||||
bag = rosbag.Bag('/home/raphael/dev/MSCKF_ws/bag/TUM/dataset-corridor1_1024_16.bag')
|
||||
for topic, msg, t in bag.read_messages(topics=[cam0_topic, cam1_topic, imu0_topic, grnd_topic]):
|
||||
|
||||
# pause if parameter set to false
|
||||
flag = False
|
||||
while not rospy.get_param('/play_bag'):
|
||||
time.sleep(0.01)
|
||||
if not flag:
|
||||
print("stopped playback")
|
||||
flag = not flag
|
||||
|
||||
if flag:
|
||||
print("resume playback")
|
||||
|
||||
if topic == cam0_topic:
|
||||
cam0_pub.publish(msg)
|
||||
|
||||
elif topic == cam1_topic:
|
||||
cam1_pub.publish(msg)
|
||||
|
||||
elif topic == imu0_topic:
|
||||
imu0_pub.publish(msg)
|
||||
|
||||
elif topic ==grnd_topic:
|
||||
grnd_pub.publish(msg)
|
||||
|
||||
#print msg
|
||||
bag.close()
|
||||
|
||||
if __name__== "__main__":
|
||||
main()
|
||||
@ -40,12 +40,10 @@ void undistortImage(
|
||||
cv::fisheye::undistortImage(src, dst, K, distortion_coeffs, K);
|
||||
else if (distortion_model == "equidistant")
|
||||
src.copyTo(dst);
|
||||
|
||||
else if (distortion_model == "pre-radtan")
|
||||
cv::undistort(src, dst, K, distortion_coeffs, K);
|
||||
else if (distortion_model == "radtan")
|
||||
src.copyTo(dst);
|
||||
|
||||
}
|
||||
|
||||
void undistortPoint(
|
||||
@ -97,7 +95,6 @@ void undistortPoint(
|
||||
pts_out.push_back(newPoint);
|
||||
}
|
||||
}
|
||||
|
||||
else if (distortion_model == "pre-equidistant" or distortion_model == "pre-radtan")
|
||||
{
|
||||
std::vector<cv::Point2f> temp_pts_out;
|
||||
|
||||
@ -42,9 +42,6 @@ ImageProcessor::~ImageProcessor() {
|
||||
}
|
||||
|
||||
bool ImageProcessor::loadParameters() {
|
||||
|
||||
// debug parameters
|
||||
nh.param<bool>("StreamPause", STREAMPAUSE, false);
|
||||
// Camera calibration parameters
|
||||
nh.param<string>("cam0/distortion_model",
|
||||
cam0.distortion_model, string("radtan"));
|
||||
@ -214,9 +211,7 @@ void ImageProcessor::stereoCallback(
|
||||
const sensor_msgs::ImageConstPtr& cam0_img,
|
||||
const sensor_msgs::ImageConstPtr& cam1_img) {
|
||||
|
||||
// stop playing bagfile if printing images
|
||||
//if(STREAMPAUSE)
|
||||
// nh.setParam("/play_bag_image", false);
|
||||
//cout << "==================================" << endl;
|
||||
|
||||
// Get the current image.
|
||||
cam0_curr_img_ptr = cv_bridge::toCvShare(cam0_img,
|
||||
@ -226,19 +221,14 @@ void ImageProcessor::stereoCallback(
|
||||
|
||||
ros::Time start_time = ros::Time::now();
|
||||
|
||||
cv::Mat newImage;
|
||||
image_handler::undistortImage(cam0_curr_img_ptr->image, newImage, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
|
||||
newImage.copyTo(cam0_curr_img_ptr->image);
|
||||
image_handler::undistortImage(cam1_curr_img_ptr->image, newImage, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
|
||||
newImage.copyTo( cam1_curr_img_ptr->image);
|
||||
|
||||
cv::Mat new_cam0;
|
||||
cv::Mat new_cam1;
|
||||
|
||||
image_handler::undistortImage(cam0_curr_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
|
||||
image_handler::undistortImage(cam1_curr_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
|
||||
|
||||
new_cam0.copyTo(cam0_curr_img_ptr->image);
|
||||
new_cam1.copyTo(cam1_curr_img_ptr->image);
|
||||
|
||||
//ROS_INFO("Publishing: %f",
|
||||
//ROS_INFO("Publishing: %f",
|
||||
// (ros::Time::now()-start_time).toSec());
|
||||
|
||||
// Build the image pyramids once since they're used at multiple places
|
||||
createImagePyramids();
|
||||
|
||||
@ -265,7 +255,6 @@ void ImageProcessor::stereoCallback(
|
||||
// Add new features into the current image.
|
||||
start_time = ros::Time::now();
|
||||
addNewFeatures();
|
||||
|
||||
//ROS_INFO("Addition time: %f",
|
||||
// (ros::Time::now()-start_time).toSec());
|
||||
|
||||
@ -294,12 +283,10 @@ void ImageProcessor::stereoCallback(
|
||||
// (ros::Time::now()-start_time).toSec());
|
||||
|
||||
// Update the previous image and previous features.
|
||||
|
||||
cam0_prev_img_ptr = cam0_curr_img_ptr;
|
||||
prev_features_ptr = curr_features_ptr;
|
||||
std::swap(prev_cam0_pyramid_, curr_cam0_pyramid_);
|
||||
|
||||
|
||||
// Initialize the current features to empty vectors.
|
||||
curr_features_ptr.reset(new GridFeatures());
|
||||
for (int code = 0; code <
|
||||
@ -307,10 +294,6 @@ void ImageProcessor::stereoCallback(
|
||||
(*curr_features_ptr)[code] = vector<FeatureMetaData>(0);
|
||||
}
|
||||
|
||||
// stop playing bagfile if printing images
|
||||
//if(STREAMPAUSE)
|
||||
// nh.setParam("/play_bag_image", true);
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
@ -607,7 +590,6 @@ void ImageProcessor::trackFeatures() {
|
||||
++after_ransac;
|
||||
}
|
||||
|
||||
|
||||
// Compute the tracking rate.
|
||||
int prev_feature_num = 0;
|
||||
for (const auto& item : *prev_features_ptr)
|
||||
@ -687,8 +669,6 @@ void ImageProcessor::stereoMatch(
|
||||
|
||||
// Further remove outliers based on the known
|
||||
// essential matrix.
|
||||
|
||||
|
||||
vector<cv::Point2f> cam0_points_undistorted(0);
|
||||
vector<cv::Point2f> cam1_points_undistorted(0);
|
||||
image_handler::undistortPoints(
|
||||
@ -698,7 +678,6 @@ void ImageProcessor::stereoMatch(
|
||||
cam1_points, cam1.intrinsics, cam1.distortion_model,
|
||||
cam1.distortion_coeffs, cam1_points_undistorted);
|
||||
|
||||
|
||||
double norm_pixel_unit = 4.0 / (
|
||||
cam0.intrinsics[0]+cam0.intrinsics[1]+
|
||||
cam1.intrinsics[0]+cam1.intrinsics[1]);
|
||||
|
||||
@ -50,7 +50,7 @@ Isometry3d CAMState::T_cam0_cam1 = Isometry3d::Identity();
|
||||
|
||||
// Static member variables in Feature class.
|
||||
FeatureIDType Feature::next_id = 0;
|
||||
double Feature::observation_noise = 0.01;
|
||||
double Feature::observation_noise = 0.2;
|
||||
Feature::OptimizationConfig Feature::optimization_config;
|
||||
|
||||
map<int, double> MsckfVio::chi_squared_test_table;
|
||||
@ -137,8 +137,6 @@ bool MsckfVio::loadParameters() {
|
||||
// Get the photometric patch size
|
||||
nh.param<int>("patch_size_n",
|
||||
N, 3);
|
||||
// Get rescaling factor for image
|
||||
nh.param<int>("image_scale", SCALE, 1);
|
||||
|
||||
// get camera information (used for back projection)
|
||||
nh.param<string>("cam0/distortion_model",
|
||||
@ -409,7 +407,6 @@ void MsckfVio::imageCallback(
|
||||
// stop playing bagfile if printing images
|
||||
if(STREAMPAUSE)
|
||||
nh.setParam("/play_bag", false);
|
||||
|
||||
// Return if the gravity vector has not been set.
|
||||
if (!is_gravity_set)
|
||||
{
|
||||
@ -427,8 +424,6 @@ void MsckfVio::imageCallback(
|
||||
// the origin.
|
||||
if (is_first_img) {
|
||||
is_first_img = false;
|
||||
|
||||
time_offset = feature_msg->header.stamp.toSec();
|
||||
state_server.imu_state.time = feature_msg->header.stamp.toSec();
|
||||
}
|
||||
static double max_processing_time = 0.0;
|
||||
@ -472,7 +467,6 @@ void MsckfVio::imageCallback(
|
||||
|
||||
|
||||
// cout << "4" << endl;
|
||||
|
||||
// Perform measurement update if necessary.
|
||||
start_time = ros::Time::now();
|
||||
removeLostFeatures();
|
||||
@ -481,7 +475,7 @@ void MsckfVio::imageCallback(
|
||||
|
||||
// cout << "5" << endl;
|
||||
start_time = ros::Time::now();
|
||||
pruneCamStateBuffer();
|
||||
pruneLastCamStateBuffer();
|
||||
double prune_cam_states_time = (
|
||||
ros::Time::now()-start_time).toSec();
|
||||
|
||||
@ -548,15 +542,14 @@ void MsckfVio::manageMovingWindow(
|
||||
cv_bridge::CvImageConstPtr cam1_img_ptr = cv_bridge::toCvShare(cam1_img,
|
||||
sensor_msgs::image_encodings::MONO8);
|
||||
|
||||
cv::Mat new_cam0;
|
||||
cv::Mat new_cam1;
|
||||
|
||||
image_handler::undistortImage(cam0_img_ptr->image, new_cam0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
|
||||
image_handler::undistortImage(cam1_img_ptr->image, new_cam1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
|
||||
cv::Mat newImage0;
|
||||
cv::Mat newImage1;
|
||||
image_handler::undistortImage(cam0_img_ptr->image, newImage0, cam0.distortion_model, cam0.intrinsics, cam0.distortion_coeffs);
|
||||
image_handler::undistortImage(cam1_img_ptr->image, newImage1, cam1.distortion_model, cam1.intrinsics, cam1.distortion_coeffs);
|
||||
|
||||
// save image information into moving window
|
||||
cam0.moving_window[state_server.imu_state.id].image = new_cam0.clone();
|
||||
cam1.moving_window[state_server.imu_state.id].image = new_cam1.clone();
|
||||
cam0.moving_window[state_server.imu_state.id].image = newImage0.clone();
|
||||
cam1.moving_window[state_server.imu_state.id].image = newImage1.clone();
|
||||
|
||||
cv::Mat xder;
|
||||
cv::Mat yder;
|
||||
@ -581,19 +574,20 @@ void MsckfVio::manageMovingWindow(
|
||||
yder/=96.;
|
||||
|
||||
|
||||
/*
|
||||
/*
|
||||
cv::Mat norm_abs_xderImage;
|
||||
cv::convertScaleAbs(xder, norm_abs_xderImage);
|
||||
cv::normalize(norm_abs_xderImage, norm_abs_xderImage, 0, 255, cv::NORM_MINMAX, CV_8UC1);
|
||||
|
||||
cv::imshow("xder", norm_abs_xderImage);
|
||||
cvWaitKey(0);
|
||||
*/
|
||||
|
||||
*/
|
||||
// save into moving window
|
||||
cam1.moving_window[state_server.imu_state.id].dximage = xder.clone();
|
||||
cam1.moving_window[state_server.imu_state.id].dyimage = yder.clone();
|
||||
|
||||
|
||||
|
||||
//TODO handle any massive overflow correctly (should be pruned, before this ever triggers)
|
||||
while(cam0.moving_window.size() > 100)
|
||||
{
|
||||
@ -793,7 +787,7 @@ void MsckfVio::mocapOdomCallback(const nav_msgs::OdometryConstPtr& msg) {
|
||||
void MsckfVio::calcErrorState()
|
||||
{
|
||||
|
||||
// imu "error"
|
||||
// imu error
|
||||
err_state_server.imu_state.id = state_server.imu_state.id;
|
||||
err_state_server.imu_state.time = state_server.imu_state.time;
|
||||
|
||||
@ -839,9 +833,9 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
|
||||
|
||||
// Counter how many IMU msgs in the buffer are used.
|
||||
int used_truth_msg_cntr = 0;
|
||||
double truth_time;
|
||||
|
||||
for (const auto& truth_msg : truth_msg_buffer) {
|
||||
truth_time = truth_msg.header.stamp.toSec();
|
||||
double truth_time = truth_msg.header.stamp.toSec();
|
||||
if (truth_time < true_state_server.imu_state.time) {
|
||||
++used_truth_msg_cntr;
|
||||
continue;
|
||||
@ -861,7 +855,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
|
||||
// Execute process model.
|
||||
processTruthtoIMU(truth_time, m_rotation, m_translation);
|
||||
++used_truth_msg_cntr;
|
||||
|
||||
}
|
||||
last_time_bound = time_bound;
|
||||
|
||||
@ -872,31 +865,6 @@ void MsckfVio::batchTruthProcessing(const double& time_bound) {
|
||||
truth_msg_buffer.erase(truth_msg_buffer.begin(),
|
||||
truth_msg_buffer.begin()+used_truth_msg_cntr);
|
||||
|
||||
std::ofstream logfile;
|
||||
|
||||
int FileHandler;
|
||||
char FileBuffer[1024];
|
||||
float load = 0;
|
||||
|
||||
FileHandler = open("/proc/loadavg", O_RDONLY);
|
||||
if( FileHandler >= 0) {
|
||||
auto file = read(FileHandler, FileBuffer, sizeof(FileBuffer) - 1);
|
||||
sscanf(FileBuffer, "%f", &load);
|
||||
close(FileHandler);
|
||||
}
|
||||
|
||||
auto gt = true_state_server.imu_state.position;
|
||||
auto gr = true_state_server.imu_state.orientation;
|
||||
logfile.open("/home/raphael/dev/MSCKF_ws/bag/log.txt", std::ios_base::app);
|
||||
//ros time, cpu load , ground truth, estimation, ros time
|
||||
logfile << true_state_server.imu_state.time - time_offset << "; " << load << "; ";
|
||||
logfile << gt[0] << "; " << gt[1] << "; " << gt[2] << "; " << gr[0] << "; " << gr[1] << "; " << gr[2] << "; " << gr[3] << ";";
|
||||
|
||||
// estimation
|
||||
auto et = state_server.imu_state.position;
|
||||
auto er = state_server.imu_state.orientation;
|
||||
logfile << et[0] << "; " << et[1] << "; " << et[2] << "; " << er[0] << "; " << er[1] << "; " << er[2] << "; " << er[3] << "; \n" << endl;;
|
||||
logfile.close();
|
||||
/*
|
||||
// calculate change
|
||||
delta_position = state_server.imu_state.position - old_imu_state.position;
|
||||
@ -1492,13 +1460,11 @@ void MsckfVio::twodotMeasurementJacobian(
|
||||
return;
|
||||
}
|
||||
|
||||
bool MsckfVio::twodotFeatureJacobian(
|
||||
void MsckfVio::twodotFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
MatrixXd& H_x, VectorXd& r)
|
||||
{
|
||||
if(FILTER != 2)
|
||||
return false;
|
||||
|
||||
const auto& feature = map_server[feature_id];
|
||||
|
||||
@ -1520,9 +1486,7 @@ bool MsckfVio::twodotFeatureJacobian(
|
||||
|
||||
MatrixXd H_xi = MatrixXd::Zero(jacobian_row_size,
|
||||
21+state_server.cam_states.size()*7);
|
||||
|
||||
MatrixXd H_yi = MatrixXd::Zero(jacobian_row_size, 1);
|
||||
|
||||
VectorXd r_i = VectorXd::Zero(jacobian_row_size);
|
||||
int stack_cntr = 0;
|
||||
|
||||
@ -1530,7 +1494,6 @@ bool MsckfVio::twodotFeatureJacobian(
|
||||
|
||||
MatrixXd H_xl;
|
||||
MatrixXd H_yl;
|
||||
|
||||
Eigen::VectorXd r_l = VectorXd::Zero(4);
|
||||
|
||||
twodotMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l);
|
||||
@ -1541,14 +1504,10 @@ bool MsckfVio::twodotFeatureJacobian(
|
||||
// Stack the Jacobians.
|
||||
H_xi.block(stack_cntr, 0, H_xl.rows(), H_xl.cols()) = H_xl;
|
||||
H_yi.block(stack_cntr, 0, H_yl.rows(), H_yl.cols()) = H_yl;
|
||||
|
||||
r_i.segment(stack_cntr, 4) = r_l;
|
||||
stack_cntr += 4;
|
||||
}
|
||||
|
||||
|
||||
H_xi.conservativeResize(stack_cntr, H_xi.cols());
|
||||
H_yi.conservativeResize(stack_cntr, H_yi.cols());
|
||||
// Project the residual and Jacobians onto the nullspace
|
||||
// of H_yj.
|
||||
|
||||
@ -1596,16 +1555,16 @@ bool MsckfVio::twodotFeatureJacobian(
|
||||
std::cout << "resume playback" << std::endl;
|
||||
nh.setParam("/play_bag", true);
|
||||
}
|
||||
return true;
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
bool MsckfVio::PhotometricPatchPointResidual(
|
||||
const StateIDType& cam_state_id,
|
||||
const Feature& feature,
|
||||
VectorXd& r)
|
||||
VectorXd& r, int patchsize)
|
||||
{
|
||||
VectorXd r_photo = VectorXd::Zero(2*N*N);
|
||||
VectorXd r_photo = VectorXd::Zero(2*patchsize*patchsize);
|
||||
int count = 0;
|
||||
const CAMState& cam_state = state_server.cam_states[cam_state_id];
|
||||
|
||||
@ -1622,6 +1581,7 @@ bool MsckfVio::PhotometricPatchPointResidual(
|
||||
estimate_photo_z_c0.push_back (estimate_irradiance_j);// *
|
||||
//estimated_illumination.frame_gain * estimated_illumination.feature_gain +
|
||||
//estimated_illumination.frame_bias + estimated_illumination.feature_bias);
|
||||
|
||||
feature.estimate_FrameIrradiance(cam_state, cam_state_id, cam1, estimate_irradiance, estimated_illumination);
|
||||
for (auto& estimate_irradiance_j : estimate_irradiance)
|
||||
estimate_photo_z_c1.push_back (estimate_irradiance_j);// *
|
||||
@ -1638,14 +1598,31 @@ bool MsckfVio::PhotometricPatchPointResidual(
|
||||
|
||||
cv::Mat current_image_c0 = cam0.moving_window.find(cam_state_id)->second.image;
|
||||
cv::Mat current_image_c1 = cam1.moving_window.find(cam_state_id)->second.image;
|
||||
for(int i = 0; i<N; i++) {
|
||||
for(int j = 0; j<N ; j++) {
|
||||
true_irradiance_c0.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c0.x + (i-(N-1)/2), p_f_c0.y + (j-(N-1)/2)), current_image_c0));
|
||||
true_irradiance_c1.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c1.x + (i-(N-1)/2), p_f_c1.y + (j-(N-1)/2)), current_image_c1));
|
||||
for(int i = 0; i<patchsize; i++) {
|
||||
for(int j = 0; j<patchsize; j++) {
|
||||
true_irradiance_c0.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c0.x + (i-(patchsize-1)/2), p_f_c0.y + (j-(patchsize-1)/2)), current_image_c0));
|
||||
true_irradiance_c1.push_back(feature.PixelIrradiance(cv::Point2f(p_f_c1.x + (i-(patchsize-1)/2), p_f_c1.y + (j-(patchsize-1)/2)), current_image_c1));
|
||||
}
|
||||
}
|
||||
|
||||
// get residual
|
||||
std::vector<Eigen::Vector3d> new_anchorPatch_3d;
|
||||
if (patchsize == 3 and N == 5)
|
||||
{
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]);
|
||||
}
|
||||
else
|
||||
{
|
||||
new_anchorPatch_3d = feature.anchorPatch_3d;
|
||||
}
|
||||
|
||||
for(auto point : feature.anchorPatch_3d)
|
||||
{
|
||||
|
||||
@ -1671,6 +1648,7 @@ bool MsckfVio::PhotometricPatchPointResidual(
|
||||
count++;
|
||||
}
|
||||
r = r_photo;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -1681,6 +1659,7 @@ bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
const Feature& feature,
|
||||
Eigen::Vector3d point,
|
||||
int count,
|
||||
int patchsize,
|
||||
Eigen::Matrix<double, 2, 1>& H_rhoj,
|
||||
Eigen::Matrix<double, 2, 6>& H_plj,
|
||||
Eigen::Matrix<double, 2, 6>& H_pAj,
|
||||
@ -1739,6 +1718,8 @@ bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
dI_dhj(1, 2) = dx_c1 * cam1.intrinsics[0];
|
||||
dI_dhj(1, 3) = dy_c1 * cam1.intrinsics[1];
|
||||
|
||||
//cout << dI_dhj(0, 0) << ", " << dI_dhj(0, 1) << endl;
|
||||
|
||||
// add jacobian
|
||||
|
||||
//dh / d{}^Cp_{ij}
|
||||
@ -1763,11 +1744,11 @@ bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
// 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[(N*N-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(N*N-1)/2].y/(rho*rho), 1/(rho*rho));
|
||||
dGpj_drhoj = -feature.T_anchor_w.linear() * Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho*rho), feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho*rho), 1/(rho*rho));
|
||||
|
||||
dGpj_XpAj.block<3, 3>(0, 0) = - feature.T_anchor_w.linear()
|
||||
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(N*N-1)/2].x/(rho),
|
||||
feature.anchorPatch_ideal[(N*N-1)/2].y/(rho),
|
||||
* skewSymmetric(Eigen::Vector3d(feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].x/(rho),
|
||||
feature.anchorPatch_ideal[(patchsize*patchsize-1)/2].y/(rho),
|
||||
1/(rho)));
|
||||
dGpj_XpAj.block<3, 3>(0, 3) = Matrix<double, 3, 3>::Identity();
|
||||
|
||||
@ -1790,41 +1771,61 @@ bool MsckfVio::PhotometricPatchPointJacobian(
|
||||
bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
const StateIDType& cam_state_id,
|
||||
const FeatureIDType& feature_id,
|
||||
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r)
|
||||
MatrixXd& H_x, MatrixXd& H_y, VectorXd& r, int patchsize)
|
||||
{
|
||||
// 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 r_photo = VectorXd::Zero(2*patchsize*patchsize);
|
||||
|
||||
// one line of the NxN Jacobians
|
||||
// one line of the patchsizexpatchsize Jacobians
|
||||
Eigen::Matrix<double, 2, 1> H_rhoj;
|
||||
Eigen::Matrix<double, 2, 6> H_plj;
|
||||
Eigen::Matrix<double, 2, 6> H_pAj;
|
||||
|
||||
Eigen::MatrixXd dI_dh(2* N*N, 4);
|
||||
Eigen::MatrixXd dI_dh(2* patchsize*patchsize, 4);
|
||||
|
||||
// combined Jacobians
|
||||
Eigen::MatrixXd H_rho(2 * N*N, 1);
|
||||
Eigen::MatrixXd H_pl(2 * N*N, 6);
|
||||
Eigen::MatrixXd H_pA(2 * N*N, 6);
|
||||
Eigen::MatrixXd H_rho(2 * patchsize*patchsize, 1);
|
||||
Eigen::MatrixXd H_pl(2 * patchsize*patchsize, 6);
|
||||
Eigen::MatrixXd H_pA(2 * patchsize*patchsize, 6);
|
||||
|
||||
// calcualte residual of patch
|
||||
if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo))
|
||||
if (not PhotometricPatchPointResidual(cam_state_id, feature, r_photo, patchsize))
|
||||
{
|
||||
return false;
|
||||
|
||||
}
|
||||
//cout << "r\n" << r_photo << endl;
|
||||
// calculate jacobian for patch
|
||||
int count = 0;
|
||||
bool valid = false;
|
||||
Matrix<double, 2, 4> dI_dhj;// = Matrix<double, 1, 2>::Zero();
|
||||
int valid_count = 0;
|
||||
for (auto point : feature.anchorPatch_3d)
|
||||
|
||||
std::vector<Eigen::Vector3d> new_anchorPatch_3d;
|
||||
if (patchsize == 3 and N == 5)
|
||||
{
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[6]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[7]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[8]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[11]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[12]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[13]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[16]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[17]);
|
||||
new_anchorPatch_3d.push_back(feature.anchorPatch_3d[18]);
|
||||
}
|
||||
else
|
||||
{
|
||||
new_anchorPatch_3d = feature.anchorPatch_3d;
|
||||
}
|
||||
|
||||
for(auto point : new_anchorPatch_3d)
|
||||
{
|
||||
// get jacobi of single point in patch
|
||||
if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, H_rhoj, H_plj, H_pAj, dI_dhj))
|
||||
if (PhotometricPatchPointJacobian(cam_state, cam_state_id, feature, point, count, patchsize, H_rhoj, H_plj, H_pAj, dI_dhj))
|
||||
{
|
||||
valid_count++;
|
||||
valid = true;
|
||||
@ -1836,7 +1837,6 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
H_pA.block<2, 6>(count*2, 0) = H_pAj;
|
||||
|
||||
dI_dh.block<2, 4>(count*2, 0) = dI_dhj;
|
||||
|
||||
count++;
|
||||
}
|
||||
// cout << "valid: " << valid_count << "/" << feature.anchorPatch_3d.size() << endl;
|
||||
@ -1847,7 +1847,7 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
MatrixXd H_xl;
|
||||
MatrixXd H_yl;
|
||||
|
||||
ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl);
|
||||
ConstructJacobians(H_rho, H_pl, H_pA, feature, cam_state_id, H_xl, H_yl, patchsize);
|
||||
|
||||
// set to return values
|
||||
H_x = H_xl;
|
||||
@ -1868,7 +1868,8 @@ bool MsckfVio::PhotometricMeasurementJacobian(
|
||||
|
||||
// visualizing functions
|
||||
feature.MarkerGeneration(marker_pub, state_server.cam_states);
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam1, r_photo, ss);
|
||||
feature.VisualizePatch(cam_state, cam_state_id, cam0, r_photo, ss);
|
||||
cout << "r\n" << r_photo << endl;
|
||||
//feature.VisualizeKernel(cam_state, cam_state_id, cam0);
|
||||
}
|
||||
|
||||
@ -1885,16 +1886,17 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
|
||||
const Feature& feature,
|
||||
const StateIDType& cam_state_id,
|
||||
Eigen::MatrixXd& H_xl,
|
||||
Eigen::MatrixXd& H_yl)
|
||||
Eigen::MatrixXd& H_yl,
|
||||
int patchsize)
|
||||
{
|
||||
H_xl = MatrixXd::Zero(2*N*N, 21+state_server.cam_states.size()*7);
|
||||
H_yl = MatrixXd::Zero(2*N*N, 1);
|
||||
H_xl = MatrixXd::Zero(2*patchsize*patchsize, 21+state_server.cam_states.size()*7);
|
||||
H_yl = MatrixXd::Zero(2*patchsize*patchsize, 1);
|
||||
|
||||
// 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, 2*N*N, 6) = H_pA;
|
||||
H_xl.block(0, 21+cam_state_cntr_anchor*7, 2*patchsize*patchsize, 6) = H_pA;
|
||||
|
||||
//get position of current frame in cam states
|
||||
auto cam_state_iter = state_server.cam_states.find(cam_state_id);
|
||||
@ -1902,7 +1904,7 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
|
||||
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, 2*N*N, 6) = H_pl;
|
||||
H_xl.block(0, 21+cam_state_cntr*7, 2*patchsize*patchsize, 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);
|
||||
@ -1922,10 +1924,9 @@ bool MsckfVio::ConstructJacobians(Eigen::MatrixXd& H_rho,
|
||||
bool MsckfVio::PhotometricFeatureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
MatrixXd& H_x, VectorXd& r)
|
||||
MatrixXd& H_x, VectorXd& r,
|
||||
int patchsize)
|
||||
{
|
||||
if(FILTER != 1)
|
||||
return false;
|
||||
|
||||
const auto& feature = map_server[feature_id];
|
||||
|
||||
@ -1959,10 +1960,8 @@ bool MsckfVio::PhotometricFeatureJacobian(
|
||||
for (const auto& cam_id : valid_cam_state_ids) {
|
||||
|
||||
// skip observation if measurement is not valid
|
||||
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l))
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if(not PhotometricMeasurementJacobian(cam_id, feature.id, H_xl, H_yl, r_l, patchsize))
|
||||
continue;
|
||||
|
||||
// set size of stacking jacobians, once the returned jacobians are known
|
||||
if(first)
|
||||
@ -2138,15 +2137,12 @@ void MsckfVio::measurementJacobian(
|
||||
return;
|
||||
}
|
||||
|
||||
bool MsckfVio::featureJacobian(
|
||||
void MsckfVio::featureJacobian(
|
||||
const FeatureIDType& feature_id,
|
||||
const std::vector<StateIDType>& cam_state_ids,
|
||||
MatrixXd& H_x, VectorXd& r)
|
||||
{
|
||||
|
||||
|
||||
if(FILTER != 0)
|
||||
return false;
|
||||
// stop playing bagfile if printing images
|
||||
|
||||
const auto& feature = map_server[feature_id];
|
||||
|
||||
@ -2205,42 +2201,16 @@ bool MsckfVio::featureJacobian(
|
||||
FullPivLU<MatrixXd> lu(H_fj.transpose());
|
||||
MatrixXd A = lu.kernel();
|
||||
|
||||
H_x = A.transpose() * H_xj;
|
||||
r = A.transpose() * r_j;
|
||||
H_x = A.transpose() * H_xj;
|
||||
r = A.transpose() * r_j;
|
||||
|
||||
Eigen::MatrixXd xres = H_x.colPivHouseholderQr().solve(r);
|
||||
// stop playing bagfile if printing images
|
||||
if(PRINTIMAGES)
|
||||
{
|
||||
|
||||
ofstream myfile;
|
||||
myfile.open("/home/raphael/dev/octave/org2octave");
|
||||
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
|
||||
<< "# name: Hx\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << H_xj.rows() << "\n"
|
||||
<< "# columns: " << H_xj.cols() << "\n"
|
||||
<< H_xj << endl;
|
||||
|
||||
myfile << "# name: Hy\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << H_fj.rows() << "\n"
|
||||
<< "# columns: " << H_fj.cols() << "\n"
|
||||
<< H_fj << endl;
|
||||
|
||||
myfile << "# name: r\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << r_j.rows() << "\n"
|
||||
<< "# columns: " << 1 << "\n"
|
||||
<< r_j << endl;
|
||||
|
||||
|
||||
myfile << "# name: xres\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << xres.rows() << "\n"
|
||||
<< "# columns: " << 1 << "\n"
|
||||
<< xres << endl;
|
||||
|
||||
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();
|
||||
|
||||
myfile.open("/home/raphael/dev/octave/org2octave");
|
||||
@ -2279,7 +2249,7 @@ bool MsckfVio::featureJacobian(
|
||||
myfile.close();
|
||||
}
|
||||
|
||||
return true;
|
||||
return;
|
||||
|
||||
}
|
||||
|
||||
@ -2416,10 +2386,8 @@ void MsckfVio::measurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
void MsckfVio::twoMeasurementUpdate(const MatrixXd& H, const VectorXd& r) {
|
||||
|
||||
|
||||
if (H.rows() == 0 || r.rows() == 0)
|
||||
{
|
||||
if (H.rows() == 0 || r.rows() == 0)
|
||||
return;
|
||||
}
|
||||
// Decompose the final Jacobian matrix to reduce computational
|
||||
// complexity as in Equation (28), (29).
|
||||
MatrixXd H_thin;
|
||||
@ -2665,12 +2633,43 @@ bool MsckfVio::gatingTest(const MatrixXd& H, const VectorXd& r, const int& dof,
|
||||
|
||||
double gamma = r.transpose() * (P1+P2).ldlt().solve(r);
|
||||
|
||||
/*
|
||||
if(gamma > 1000000)
|
||||
{
|
||||
cout << " logging " << endl;
|
||||
ofstream myfile;
|
||||
myfile.open("/home/raphael/dev/octave/log2octave");
|
||||
myfile << "# Created by Octave 3.8.1, Wed Jun 12 14:36:37 2019 CEST <raphael@raphael-desktop>\n"
|
||||
<< "# name: H\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << H.rows() << "\n"
|
||||
<< "# columns: " << H.cols() << "\n"
|
||||
<< H << endl;
|
||||
|
||||
myfile << "# name: r\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << r.rows() << "\n"
|
||||
<< "# columns: " << 1 << "\n"
|
||||
<< r << endl;
|
||||
|
||||
myfile << "# name: C\n"
|
||||
<< "# type: matrix\n"
|
||||
<< "# rows: " << state_server.state_cov.rows() << "\n"
|
||||
<< "# columns: " << state_server.state_cov.cols() << "\n"
|
||||
<< state_server.state_cov << endl;
|
||||
myfile.close();
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
|
||||
if (chi_squared_test_table[dof] == 0)
|
||||
return false;
|
||||
if (gamma < chi_squared_test_table[dof]) {
|
||||
// cout << "passed" << endl;
|
||||
if(filter == 1)
|
||||
cout << "gate: " << dof << " " << gamma << " " <<
|
||||
chi_squared_test_table[dof] << endl;
|
||||
// cout << "passed" << endl;
|
||||
return true;
|
||||
} else {
|
||||
// cout << "failed" << endl;
|
||||
@ -2683,6 +2682,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
// BTW, find the size the final Jacobian matrix and residual vector.
|
||||
int jacobian_row_size = 0;
|
||||
int pjacobian_row_size = 0;
|
||||
int p2jacobian_row_size = 0;
|
||||
int twojacobian_row_size = 0;
|
||||
|
||||
vector<FeatureIDType> invalid_feature_ids(0);
|
||||
@ -2724,6 +2724,7 @@ void MsckfVio::removeLostFeatures() {
|
||||
}
|
||||
|
||||
pjacobian_row_size += 2*N*N*feature.observations.size();
|
||||
p2jacobian_row_size += 2*3*3*feature.observations.size();
|
||||
twojacobian_row_size += 4*feature.observations.size();
|
||||
jacobian_row_size += 4*feature.observations.size() - 3;
|
||||
|
||||
@ -2752,6 +2753,10 @@ void MsckfVio::removeLostFeatures() {
|
||||
VectorXd pr = VectorXd::Zero(pjacobian_row_size);
|
||||
int pstack_cntr = 0;
|
||||
|
||||
MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size,
|
||||
21+7*state_server.cam_states.size());
|
||||
VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
|
||||
int p2stack_cntr = 0;
|
||||
|
||||
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size,
|
||||
21+7*state_server.cam_states.size());
|
||||
@ -2768,12 +2773,17 @@ void MsckfVio::removeLostFeatures() {
|
||||
|
||||
MatrixXd H_xj;
|
||||
VectorXd r_j;
|
||||
MatrixXd pH_xj;
|
||||
VectorXd pr_j;
|
||||
|
||||
MatrixXd twoH_xj;
|
||||
VectorXd twor_j;
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j))
|
||||
MatrixXd pH_xj;
|
||||
VectorXd pr_j;
|
||||
|
||||
MatrixXd p2H_xj;
|
||||
VectorXd p2r_j;
|
||||
|
||||
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, pH_xj, pr_j, N))
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, cam_state_ids.size()-1)) {
|
||||
pH_x.block(pstack_cntr, 0, pH_xj.rows(), pH_xj.cols()) = pH_xj;
|
||||
@ -2782,25 +2792,31 @@ void MsckfVio::removeLostFeatures() {
|
||||
}
|
||||
}
|
||||
|
||||
if(featureJacobian(feature.id, cam_state_ids, H_xj, r_j))
|
||||
/*
|
||||
cout << "3: " << endl;
|
||||
if(PhotometricFeatureJacobian(feature.id, cam_state_ids, p2H_xj, p2r_j, 3))
|
||||
{
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
|
||||
cout << p2H_x.rows() << ":" << p2H_x.cols() << ", " << p2H_xj.rows() << ":" << p2H_xj.cols() << endl;
|
||||
p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
|
||||
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
|
||||
p2stack_cntr += p2H_xj.rows();
|
||||
}
|
||||
}
|
||||
*/
|
||||
featureJacobian(feature.id, cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j);
|
||||
|
||||
if(twodotFeatureJacobian(feature.id, cam_state_ids, twoH_xj, twor_j))
|
||||
{
|
||||
if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
}
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
}
|
||||
if (gatingTest(twoH_xj, twor_j, twor_j.size())) { //, cam_state_ids.size()-1)) {
|
||||
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
}
|
||||
|
||||
|
||||
|
||||
// Put an upper bound on the row size of measurement Jacobian,
|
||||
// which helps guarantee the executation time.
|
||||
@ -2813,21 +2829,15 @@ void MsckfVio::removeLostFeatures() {
|
||||
pr.conservativeResize(pstack_cntr);
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
}
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
|
||||
if(stack_cntr)
|
||||
{
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
measurementUpdate(H_x, r);
|
||||
|
||||
}
|
||||
if(twostack_cntr)
|
||||
{
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
}
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
|
||||
// Perform the measurement update step.
|
||||
measurementUpdate(H_x, r);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
|
||||
// Remove all processed features from the map.
|
||||
for (const auto& feature_id : processed_feature_ids)
|
||||
@ -2891,6 +2901,7 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
// Set the size of the Jacobian matrix.
|
||||
int jacobian_row_size = 0;
|
||||
int pjacobian_row_size = 0;
|
||||
int p2jacobian_row_size = 0;
|
||||
int twojacobian_row_size = 0;
|
||||
|
||||
|
||||
@ -2929,6 +2940,7 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
}
|
||||
|
||||
pjacobian_row_size += 2*N*N*feature.observations.size();
|
||||
p2jacobian_row_size += 2*3*3*feature.observations.size();
|
||||
jacobian_row_size += 4*feature.observations.size() - 3;
|
||||
twojacobian_row_size += 4*feature.observations.size();
|
||||
|
||||
@ -2944,11 +2956,16 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
VectorXd pr = VectorXd::Zero(pjacobian_row_size);
|
||||
MatrixXd twoH_xj;
|
||||
VectorXd twor_j;
|
||||
MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size());
|
||||
VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
|
||||
MatrixXd p2H_xj;
|
||||
VectorXd p2r_j;
|
||||
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
|
||||
VectorXd twor = VectorXd::Zero(twojacobian_row_size);
|
||||
int stack_cntr = 0;
|
||||
int pruned_cntr = 0;
|
||||
int pstack_cntr = 0;
|
||||
int p2stack_cntr = 0;
|
||||
int twostack_cntr = 0;
|
||||
|
||||
for (auto& item : map_server) {
|
||||
@ -2964,10 +2981,8 @@ 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, N))
|
||||
{
|
||||
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) { //, 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;
|
||||
@ -2975,23 +2990,31 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
}
|
||||
}
|
||||
|
||||
if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
|
||||
/*
|
||||
cout << "3: " << endl;
|
||||
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
|
||||
{
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
pruned_cntr++;
|
||||
}
|
||||
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
|
||||
p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
|
||||
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
|
||||
p2stack_cntr += p2H_xj.rows();
|
||||
}
|
||||
}
|
||||
*/
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
|
||||
if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
|
||||
{
|
||||
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
|
||||
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
if (gatingTest(H_xj, r_j, r_j.size())) {// involved_cam_state_ids.size())) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
pruned_cntr++;
|
||||
}
|
||||
|
||||
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
|
||||
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
}
|
||||
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
@ -3005,19 +3028,15 @@ void MsckfVio::pruneLastCamStateBuffer()
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
}
|
||||
|
||||
if(stack_cntr)
|
||||
{
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
measurementUpdate(H_x, r);
|
||||
}
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
|
||||
if(twostack_cntr)
|
||||
{
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
}
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
|
||||
// Perform the measurement update step.
|
||||
measurementUpdate(H_x, r);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
|
||||
//reduction
|
||||
int cam_sequence = std::distance(state_server.cam_states.begin(),
|
||||
@ -3070,6 +3089,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
// Find the size of the Jacobian matrix.
|
||||
int jacobian_row_size = 0;
|
||||
int pjacobian_row_size = 0;
|
||||
int p2jacobian_row_size = 0;
|
||||
int twojacobian_row_size = 0;
|
||||
|
||||
for (auto& item : map_server) {
|
||||
@ -3117,6 +3137,7 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
|
||||
twojacobian_row_size += 4*involved_cam_state_ids.size();
|
||||
pjacobian_row_size += 2*N*N*involved_cam_state_ids.size();
|
||||
p2jacobian_row_size += 2*3*3*involved_cam_state_ids.size();
|
||||
jacobian_row_size += 4*involved_cam_state_ids.size() - 3;
|
||||
}
|
||||
|
||||
@ -3128,11 +3149,19 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
MatrixXd H_x = MatrixXd::Zero(jacobian_row_size, 21+7*state_server.cam_states.size());
|
||||
VectorXd r = VectorXd::Zero(jacobian_row_size);
|
||||
int stack_cntr = 0;
|
||||
|
||||
MatrixXd pH_xj;
|
||||
VectorXd pr_j;
|
||||
MatrixXd pH_x = MatrixXd::Zero(pjacobian_row_size, 21+7*state_server.cam_states.size());
|
||||
VectorXd pr = VectorXd::Zero(pjacobian_row_size);
|
||||
int pstack_cntr = 0;
|
||||
|
||||
MatrixXd p2H_x = MatrixXd::Zero(p2jacobian_row_size, 21+7*state_server.cam_states.size());
|
||||
VectorXd p2r = VectorXd::Zero(p2jacobian_row_size);
|
||||
MatrixXd p2H_xj;
|
||||
VectorXd p2r_j;
|
||||
int p2stack_cntr = 0;
|
||||
|
||||
MatrixXd twoH_xj;
|
||||
VectorXd twor_j;
|
||||
MatrixXd twoH_x = MatrixXd::Zero(twojacobian_row_size, 21+7*state_server.cam_states.size());
|
||||
@ -3151,31 +3180,43 @@ 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(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, pH_xj, pr_j, N))
|
||||
{
|
||||
if (gatingTest(pH_xj, pr_j, pr_j.size(), 1)) {// involved_cam_state_ids.size())) {
|
||||
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();
|
||||
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();
|
||||
}
|
||||
}
|
||||
if(featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j))
|
||||
|
||||
/*
|
||||
cout << "3: " << endl;
|
||||
if(PhotometricFeatureJacobian(feature.id, involved_cam_state_ids, p2H_xj, p2r_j, 3))
|
||||
{
|
||||
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
if (gatingTest(p2H_xj, p2r_j, p2r_j.size(), 1)) { //, cam_state_ids.size()-1)) {
|
||||
p2H_x.block(p2stack_cntr, 0, p2H_xj.rows(), p2H_xj.cols()) = p2H_xj;
|
||||
p2r.segment(p2stack_cntr, p2r_j.rows()) = p2r_j;
|
||||
p2stack_cntr += p2H_xj.rows();
|
||||
}
|
||||
}
|
||||
if(twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j))
|
||||
{
|
||||
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
|
||||
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
}
|
||||
*/
|
||||
|
||||
featureJacobian(feature.id, involved_cam_state_ids, H_xj, r_j);
|
||||
twodotFeatureJacobian(feature.id, involved_cam_state_ids, twoH_xj, twor_j);
|
||||
|
||||
if (gatingTest(H_xj, r_j, involved_cam_state_ids.size())) {// involved_cam_state_ids.size())) {
|
||||
H_x.block(stack_cntr, 0, H_xj.rows(), H_xj.cols()) = H_xj;
|
||||
r.segment(stack_cntr, r_j.rows()) = r_j;
|
||||
stack_cntr += H_xj.rows();
|
||||
}
|
||||
|
||||
if (gatingTest(twoH_xj, twor_j, twor_j.size())) {// involved_cam_state_ids.size())) {
|
||||
twoH_x.block(twostack_cntr, 0, twoH_xj.rows(), twoH_xj.cols()) = twoH_xj;
|
||||
twor.segment(twostack_cntr, twor_j.rows()) = twor_j;
|
||||
twostack_cntr += twoH_xj.rows();
|
||||
}
|
||||
|
||||
|
||||
for (const auto& cam_id : involved_cam_state_ids)
|
||||
feature.observations.erase(cam_id);
|
||||
|
||||
@ -3188,19 +3229,15 @@ void MsckfVio::pruneCamStateBuffer() {
|
||||
photometricMeasurementUpdate(pH_x, pr);
|
||||
}
|
||||
|
||||
if(stack_cntr)
|
||||
{
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
measurementUpdate(H_x, r);
|
||||
}
|
||||
H_x.conservativeResize(stack_cntr, H_x.cols());
|
||||
r.conservativeResize(stack_cntr);
|
||||
|
||||
if(stack_cntr)
|
||||
{
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
}
|
||||
twoH_x.conservativeResize(twostack_cntr, twoH_x.cols());
|
||||
twor.conservativeResize(twostack_cntr);
|
||||
|
||||
// Perform the measurement update step.
|
||||
measurementUpdate(H_x, r);
|
||||
twoMeasurementUpdate(twoH_x, twor);
|
||||
|
||||
//reduction
|
||||
for (const auto& cam_id : rm_cam_state_ids) {
|
||||
|
||||
Reference in New Issue
Block a user