10000 add image image_variance · gaowenliang/vins_so@2b60e32 · GitHub
[go: up one dir, main page]
Skip to content

Commit

Permalink
add image image_variance
Browse files Browse the repository at this point in the history
  • Loading branch information
@gaowenliang
gaowenliang committed May 2, 2018
1 parent 63e5e7d commit 2b60e32
Show file tree
Hide file tree
Showing 11 changed files with 61 additions and 62 deletions.
4 changes: 2 additions & 2 deletions include/vins_so/estimator/factor/ProjectionFactorMultiCam.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
class ProjectionFactorMultiCam : public ceres::SizedCostFunction< 2, 7, 7, 1, 7, 7 >
{
public:
ProjectionFactorMultiCam( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j );
ProjectionFactorMultiCam( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j, const double& err );

virtual bool Evaluate( double const* const* parameters, double* residuals, double** jacobians ) const;

Expand All @@ -20,7 +20,7 @@ class ProjectionFactorMultiCam : public ceres::SizedCostFunction< 2, 7, 7, 1, 7,
public:
Eigen::Vector3d pts_i, pts_j;
Eigen::Matrix< double, 2, 3 > tangent_base;
static Eigen::Matrix2d sqrt_info;
Eigen::Matrix2d sqrt_info;
static double sum_t;
};

Expand Down
4 changes: 2 additions & 2 deletions include/vins_so/estimator/factor/ProjectionFactorSingleCam.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
class ProjectionFactorSingleCam : public ceres::SizedCostFunction< 2, 7, 7, 1, 7 >
{
public:
ProjectionFactorSingleCam( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j );
ProjectionFactorSingleCam( const Eigen::Vector3d& _pts_i, const Eigen::Vector3d& _pts_j, const double& err );

virtual bool Evaluate( double const* const* parameters, double* residuals, double** jacobians ) const;

Expand All @@ -20,7 +20,7 @@ class ProjectionFactorSingleCam : public ceres::SizedCostFunction< 2, 7, 7, 1, 7
public:
Eigen::Vector3d pts_i, pts_j;
Eigen::Matrix< double, 2, 3 > tangent_base;
static Eigen::Matrix2d sqrt_info;
Eigen::Matrix2d sqrt_info;
static double sum_t;
};

Expand Down
2 changes: 1 addition & 1 deletion include/vins_so/estimator/slideWindow/slidewindowpose.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,7 +19,7 @@ class SlideWindowPose : public SlideWindowBase
void slideWindow( const bool shift_depth, const std::vector< Tf > tf_ic );

MarginalizationFlag addFeaturesToWindow( int _frame_count, //
const map< int, vector< pair< int, Vector3d > > >& _image );
const FeatureData& _image );

void slideWindowOld( const Tf& back_Tf_wi, const std::vector< Tf >& Tf_ic, bool shift_depth );
void slideWindowNew( );
Expand Down
18 changes: 11 additions & 7 deletions include/vins_so/type/featuretype.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,6 +11,12 @@ using namespace Eigen;

struct VisualMeas
{
VisualMeas( const int _cam_id, const Eigen::Vector3d& _pt )
: cam_id( _cam_id )
, err( 0.0 )
, pt( _pt )
{
}
VisualMeas( const int _cam_id, const double& _err, const Eigen::Vector3d& _pt )
: cam_id( _cam_id )
, err( _err )
Expand All @@ -23,9 +29,7 @@ struct VisualMeas
Eigen::Vector3d pt; // measurement baer vector
};

typedef std::map< int, std::vector< VisualMeas > > FeatureErrData;

typedef std::map< int, std::vector< std::pair< int, Eigen::Vector3d > > > FeatureData;
typedef std::map< int, std::vector< VisualMeas > > FeatureData;
typedef std::vector< pair< Eigen::Vector3d, Eigen::Vector3d > > PointsCorres;

class FeaturePerCamera
Expand All @@ -36,16 +40,16 @@ class FeaturePerCamera
, m_measPoint( _point.normalized( ) )
{
}
FeaturePerCamera( int _camera_id, const Vector4d& _point )
FeaturePerCamera( int _camera_id, const Vector3d& _point, const double& _error )
: m_cameraId( _camera_id )
, m_measPoint( Vector3d( _point( 0 ), _point( 1 ), _point( 2 ) ).normalized( ) )
, m_error_angle( _point( 3 ) )
, m_measPoint( _point.normalized( ) )
, m_errAngle( _error )
{
}

int m_cameraId;
Vector3d m_measPoint;
double m_error_angle;
double m_errAngle;

double m_parallax;
double dep_gradient;
Expand Down
24 changes: 12 additions & 12 deletions src/estimator/estimator.cpp
View file
E864
Original file line number Diff line number Diff line change
Expand Up @@ -71,12 +71,6 @@ Estimator::setParameter( )

vioInitialSys->setEx( ex_ics );
}

ProjectionFactorSingleCam::sqrt_info = 460 / 2.5 * Matrix2d::Identity( );
ProjectionFactorMultiCam::sqrt_info = 460 / 2.5 * Matrix2d::Identity( );

ROS_DEBUG_STREAM( "ProjectionFactorSingleCam::sqrt_info\n"
<< ProjectionFactorSingleCam::sqrt_info );
}

void
Expand Down Expand Up @@ -489,9 +483,11 @@ Estimator::optimization( )
{
if ( camera_id_i == it_per_cam.m_cameraId )
{
Vector3d pts_j = it_per_cam.m_measPoint;
Vector3d pts_j = it_per_cam.m_measPoint;
double errAngle = it_per_cam.m_errAngle;

ProjectionFactorSingleCam* f = new ProjectionFactorSingleCam( pts_i, pts_j );
ProjectionFactorSingleCam* f
= new ProjectionFactorSingleCam( pts_i, pts_j, errAngle );

problem.AddResidualBlock( f,
loss_function,
Expand All @@ -505,8 +501,9 @@ Estimator::optimization( )
{
Vector3d pts_j = it_per_cam.m_measPoint;
int camera_id_j = it_per_cam.m_cameraId;
double errAngle = it_per_cam.m_errAngle;

ProjectionFactorMultiCam* f = new ProjectionFactorMultiCam( pts_i, pts_j );
ProjectionFactorMultiCam* f = new ProjectionFactorMultiCam( pts_i, pts_j, errAngle );
problem.AddResidualBlock( f,
loss_function,
paraPose[imu_i],
Expand Down Expand Up @@ -625,10 +622,11 @@ Estimator::optimization( )
{
if ( camera_id_i == it_per_cam.m_cameraId )
{
Vector3d pts_j = it_per_cam.m_measPoint;
Vector3d pts_j = it_per_cam.m_measPoint;
double errAngle = it_per_cam.m_errAngle;

ProjectionFactorSingleCam* f
= new ProjectionFactorSingleCam( pts_i, pts_j );
= new ProjectionFactorSingleCam( pts_i, pts_j, errAngle );

ResidualBlockInfo* residual_block_info
= new ResidualBlockInfo( f,
Expand All @@ -646,8 +644,10 @@ Estimator::optimization( )
{
Vector3d pts_j = it_per_cam.m_measPoint;
int camera_id_j = it_per_cam.m_cameraId;
double errAngle = it_per_cam.m_errAngle;

ProjectionFactorMultiCam* f = new ProjectionFactorMultiCam( pts_i, pts_j );
ProjectionFactorMultiCam* f
= new ProjectionFactorMultiCam( pts_i, pts_j, errAngle );

ResidualBlockInfo* residual_block_info
= new ResidualBlockInfo( f,
Expand Down
7 changes: 5 additions & 2 deletions src/estimator/factor/ProjectionFactorMultiCam.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,16 @@
#include "vins_so/estimator/factor/ProjectionFactorMultiCam.h"

Eigen::Matrix2d ProjectionFactorMultiCam::sqrt_info;
double ProjectionFactorMultiCam::sum_t;

ProjectionFactorMultiCam::ProjectionFactorMultiCam( const Eigen::Vector3d& _pts_i,
const Eigen::Vector3d& _pts_j )
const Eigen::Vector3d& _pts_j,
const double& err )
: pts_i( _pts_i )
, pts_j( _pts_j )
{

sqrt_info = 1.5 / err * Eigen::Matrix2d::Identity( );

#ifdef UNIT_SPHERE_ERROR
Eigen::Vector3d b1, b2;
Eigen::Vector3d a = pts_j.normalized( );
Expand Down
5 changes: 3 additions & 2 deletions src/estimator/factor/ProjectionFactorSingleCam.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,13 +1,14 @@
#include "vins_so/estimator/factor/ProjectionFactorSingleCam.h"

Eigen::Matrix2d ProjectionFactorSingleCam::sqrt_info;
double ProjectionFactorSingleCam::sum_t;

ProjectionFactorSingleCam::ProjectionFactorSingleCam( const Eigen::Vector3d& _pts_i,
const Eigen::Vector3d& _pts_j )
const Eigen::Vector3d& _pts_j,
const double& err )
: pts_i( _pts_i )
, pts_j( _pts_j )
{
sqrt_info = 1.5 / err * Eigen::Matrix2d::Identity( );

#ifdef UNIT_SPHERE_ERROR
Eigen::Vector3d b1, b2;
Expand Down
29 changes: 18 additions & 11 deletions src/estimator/feature_manager/feature_manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,12 +102,13 @@ FeatureManager::addFeature( int frame_count, const FeatureData& image )
vector< FeaturePerCamera > f_per_cam;
for ( auto& i_p : id_pts.second )
{
f_per_cam.push_back( FeaturePerCamera( i_p.first, i_p.second ) );
f_per_cam.push_back( FeaturePerCamera( i_p.cam_id, i_p.pt ) );
}
// std::cout << " stereo " << f_per_cam.size( ) << std::endl;
FeaturePerFrame f_per_fra( f_per_cam );

int feature_id = id_pts.first;

auto it = find_if( feature.begin( ), feature.end( ), [feature_id]( const FeaturePerId& it ) {
return it.m_featureId == feature_id;
} );
Expand Down Expand Up @@ -140,11 +141,11 @@ FeatureManager::addFeatureCamIndex( int frame_count, const FeatureData& image, i
vector< FeaturePerCamera > f_per_cam;
for ( auto& i_p : id_pts.second )
{
int camera_id = i_p.first;
int camera_id = i_p.cam_id;
if ( camera_id != camera_index )
continue;

f_per_cam.push_back( FeaturePerCamera( camera_id, i_p.second ) );
f_per_cam.push_back( FeaturePerCamera( camera_id, i_p.pt ) );
}
// std::cout << " stereo " << f_per_cam.size( ) << std::endl;
FeaturePerFrame f_per_fra( f_per_cam );
Expand Down Expand Up @@ -184,17 +185,20 @@ FeatureManager::addFeatureStereo( int frame_count, const FeatureData& image )
vector< FeaturePerCamera > f_per_cam;
for ( auto& i_p : id_pts.second )
{
f_per_cam.push_back( FeaturePerCamera( i_p.first, i_p.second ) );
f_per_cam.push_back( FeaturePerCamera( i_p.cam_id, i_p.pt ) );
}
FeaturePerFrame f_per_fra( f_per_cam );

if ( f_per_fra.m_numOfMeas == 1 )
continue;

int feature_id = id_pts.first;
auto it = find_if( feature.begin( ), feature.end( ), [feature_id]( const FeaturePerId& it ) {
return it.m_featureId == feature_id;
} );

auto it = find_if( feature.begin( ), //
feature.end( ),
[feature_id]( const FeaturePerId& it ) {
return it.m_featureId == feature_id;
} );

if ( it == feature.end( ) )
{
Expand Down Expand Up @@ -230,15 +234,18 @@ FeatureManager::addFeatureCheckParallax( int frame_count, const FeatureData& ima
vector< FeaturePerCamera > f_per_cam;
for ( auto& i_p : id_pts.second )
{
f_per_cam.push_back( FeaturePerCamera( i_p.first, i_p.second ) );
f_per_cam.push_back( FeaturePerCamera( i_p.cam_id, i_p.pt, i_p.err ) );
}
// std::cout << " stereo " << f_per_cam.size( ) << std::endl;
FeaturePerFrame f_per_fra( f_per_cam );

int feature_id = id_pts.first;
auto it = find_if( feature.begin( ), feature.end( ), [feature_id]( const FeaturePerId& it ) {
return it.m_featureId == feature_id;
} );

auto it = find_if( feature.begin( ), //
feature.end( ),
[feature_id]( const FeaturePerId& it ) {
return it.m_featureId == feature_id;
} );

if ( it == feature.end( ) )
{
Expand Down
6 changes: 3 additions & 3 deletions src/estimator/initial_lib/InitialMono/initialmonovio.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,11 +72,11 @@ InitVio::InitialMonoVio::getFeatureCamIndex( int cam_index, //
for ( auto& cap_feature : _points )
{
int feature_id = cap_feature.first;
int camera_id = cap_feature.second.at( 0 ).first;
int camera_id = cap_feature.second.at( 0 ).cam_id;

if ( camera_id == cam_index )
{
Vector3d means_point = cap_feature.second.at( 0 ).second;
Vector3d means_point = cap_feature.second.at( 0 ).pt;

if ( means_point.z( ) > z_the )
cap_image_index[feature_id].emplace_back( camera_id, means_point );
Expand Down Expand Up @@ -251,7 +251,7 @@ InitVio::InitialMonoVio::initialStructure( )
Vector3d world_pts = it->second;
pts_3_vector_tmp.push_back( world_pts );

Vector3d img_pts = i_p.second.head< 3 >( );
Vector3d img_pts = i_p.pt;
pts_2_vector_tmp.push_back( img_pts );
}
}
Expand Down
2 changes: 1 addition & 1 deletion src/estimator/slideWindow/slidewindowpose.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -69,7 +69,7 @@ SlideWindowPose::slideWindow( const bool shift_depth, const std::vector< Tf > tf

MarginalizationFlag
SlideWindowPose::addFeaturesToWindow( int _frame_count, //
const map< int, vector< pair< int, Vector3d > > >& _image )
const FeatureData& _image )
{
ROS_DEBUG( "Adding feature points %lu", _image.size( ) );

Expand Down
22 changes: 3 additions & 19 deletions src/estimator_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -241,35 +241,19 @@ process( )
ROS_DEBUG( "processing vision data with stamp %f \n", img_msg->header.stamp.toSec( ) );

TicToc t_s;
// FeatureData image; // map< int, vector< pair< int, Vector3d > > >
FeatureData image; // map< int, vector< pair< int, Vector3d > > >
FeatureErrData image2; // map< int, vector< pair< int, Vector3d > > >
FeatureData image; // map< int, vector< pair< int, Vector3d > > >
for ( unsigned int i = 0; i < img_msg->points.size( ); i++ )
{
int feature_id = img_msg->channels[0].values[i] + 0.5; // feature id
int camera_id = img_msg->channels[1].values[i]; // camera id
double error = img_msg->channels[2].values[i];
ROS_DEBUG_STREAM( "error " << error );
// int feature_id = v;

// image[feature_id].emplace_back( camera_id,
// Vector3d( img_msg->points[i].x, //
// img_msg->points[i].y,
// img_msg->points[i].z )
// .normalized( ) );
double error = img_msg->channels[2].values[i]; // error angle

image[feature_id].emplace_back( camera_id,
error,
Vector3d( img_msg->points[i].x, //
img_msg->points[i].y,
img_msg->points[i].z )
.normalized( ) );

image2[feature_id].emplace_back( camera_id,
error,
Vector3d( img_msg->points[i].x, //
img_msg->points[i].y,
img_msg->points[i].z )
.normalized( ) );
}
estimator.processImage( image, img_msg->header );

Expand Down

0 comments on commit 2b60e32

Please sign in to comment.
0