public override void func_zeroedyi_and_dzeroedyi_by_dxp_and_dzeroedyi_by_dyi(Vector yi, Vector xp)
{
ThreeD_Motion_Model mm = ((Wide_Camera_Point_Feature_Measurement_Model)wide_model).threed_motion_model;
// Extract cartesian and quaternion components of xp
mm.func_r(xp);
mm.func_q(xp);
Vector3D yWiminusrW = new Vector3D((new Vector3D(yi)) - (mm.get_rRES()));
Quaternion qRES = mm.get_qRES();
Quaternion qRW = qRES.Inverse();
MatrixFixed dqRW_by_dq = MatrixFixed.dqbar_by_dq();
// Rotation RRW
RotationMatrix RRW = qRW.RotationMatrix();
// Position of feature relative to robot in robot frame
Vector3D zeroedyi = new Vector3D(RRW * yWiminusrW);
zeroedyiRES.Update(zeroedyi.GetVNL3());
// Now calculate Jacobians
// dzeroedyi_by_dyi is RRW
dzeroedyi_by_dyiRES.Update(RRW);
// dzeroedyi_by_dxp has 2 partitions:
// dzeroedyi_by_dr (3 * 3)
// dzeroedyi_by_dq (3 * 4)
MatrixFixed dzeroedyi_by_dr = (MatrixFixed)(RRW);
dzeroedyi_by_dr *= -1.0f;
//These should be 3x4 dimension
MatrixFixed dzeroedyi_by_dqRW = MatrixFixed.dRq_times_a_by_dq(qRW, yWiminusrW);
MatrixFixed dzeroedyi_by_dq = dzeroedyi_by_dqRW * dqRW_by_dq;
dzeroedyi_by_dxpRES.Update(dzeroedyi_by_dr, 0, 0);
dzeroedyi_by_dxpRES.Update(dzeroedyi_by_dq, 0, 3);
}
// Function to find a region in an image guided by current motion prediction
// which doesn't overlap existing features
public static bool FindNonOverlappingRegion(Scene_Single scene,
Vector local_u,
float delta_t,
Partially_Initialised_Feature_Measurement_Model default_feature_type_for_initialisation,
uint camera_width,
uint camera_height,
uint BOXSIZE,
ref int init_feature_search_ustart,
ref int init_feature_search_vstart,
ref int init_feature_search_ufinish,
ref int init_feature_search_vfinish,
uint FEATURE_INIT_STEPS_TO_PREDICT,
float FEATURE_INIT_DEPTH_HYPOTHESIS,
Random rnd)
{
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
Vector local_xv = new Vector(scene.get_xv());
for (uint i = 0; i < FEATURE_INIT_STEPS_TO_PREDICT; i++)
{
scene.get_motion_model().func_fv_and_dfv_by_dxv(local_xv, local_u, delta_t);
local_xv.Update(scene.get_motion_model().get_fvRES());
}
threed_motion_model.func_xp(local_xv);
Vector local_xp = new Vector(threed_motion_model.get_xpRES());
threed_motion_model.func_r(local_xp);
Vector3D rW = threed_motion_model.get_rRES();
threed_motion_model.func_q(local_xp);
Quaternion qWR = threed_motion_model.get_qRES();
// yW = rW + RWR hR
Vector3D hR = new Vector3D(0.0f, 0.0f, FEATURE_INIT_DEPTH_HYPOTHESIS);
// Used Inverse + transpose because this was't compiling the normal way
Vector3D yW = new Vector3D(rW + qWR.RotationMatrix() * hR);
// Then project that point into the current camera position
scene.get_motion_model().func_xp(scene.get_xv());
Fully_Initialised_Feature_Measurement_Model fully_init_fmm =
(Fully_Initialised_Feature_Measurement_Model)(default_feature_type_for_initialisation.more_initialised_feature_measurement_model);
Vector yWVNL = yW.GetVNL3();
fully_init_fmm.func_hi_and_dhi_by_dxp_and_dhi_by_dyi(yWVNL, scene.get_motion_model().get_xpRES());
// Now, this defines roughly how much we expect a feature initialised to move
float suggested_u = fully_init_fmm.get_hiRES()[0];
float suggested_v = fully_init_fmm.get_hiRES()[1];
float predicted_motion_u = camera_width / 2.0f - suggested_u;
float predicted_motion_v = camera_height / 2.0f - suggested_v;
// So, the limits of a "safe" region within which we can initialise
// features so that they end up staying within the screen
// (Making the approximation that the whole screen moves like the
// centre point)
int safe_feature_search_ustart = (int)(-predicted_motion_u);
int safe_feature_search_vstart = (int)(-predicted_motion_v);
int safe_feature_search_ufinish = (int)(camera_width - predicted_motion_u);
int safe_feature_search_vfinish = (int)(camera_height - predicted_motion_v);
if (safe_feature_search_ustart < ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_ustart = (int)((BOXSIZE - 1) / 2 + 1);
}
if (safe_feature_search_ufinish > (int)camera_width - ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_ufinish = (int)(camera_width - (BOXSIZE - 1) / 2 - 1);
}
if (safe_feature_search_vstart < ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_vstart = (int)((BOXSIZE - 1) / 2 + 1);
}
if (safe_feature_search_vfinish > (int)camera_height - ((int)((BOXSIZE - 1) / 2) + 1))
{
safe_feature_search_vfinish = (int)(camera_height - (BOXSIZE - 1) / 2 - 1);
}
return(FindNonOverlappingRegionNoPredict(safe_feature_search_ustart,
safe_feature_search_vstart,
safe_feature_search_ufinish,
safe_feature_search_vfinish,
scene,
ref init_feature_search_ustart,
ref init_feature_search_vstart,
ref init_feature_search_ufinish,
ref init_feature_search_vfinish, rnd));
}
public void SetUp3DDisplays()
{
// Set display virtual camera parameters to match those of the real camera
// being used in MonoSLAM
Partially_Initialised_Feature_Measurement_Model default_pifmm =
monoslaminterface.GetDefaultFeatureTypeForInitialisation();
if (default_pifmm != null)
{
Line_Init_Wide_Point_Feature_Measurement_Model default_wide_pifmm =
(Line_Init_Wide_Point_Feature_Measurement_Model)default_pifmm;
Graphics_Fku = default_wide_pifmm.get_camera().Fku();
Graphics_Fkv = default_wide_pifmm.get_camera().Fkv();
Graphics_U0 = default_wide_pifmm.get_camera().U0();
Graphics_V0 = default_wide_pifmm.get_camera().V0();
Graphics_Kd1 = default_wide_pifmm.get_camera().Kd1();
// First display for external 3D view
/*
* threedtool = new ThreeDToolGlowWidget(this,
* controlpanel1->Width() + controlpanel2->Width(), 0,
* monoslaminterface->GetCameraWidth(),
* monoslaminterface->GetCameraHeight());
* threedtool->DrawEvent.Attach(this, &MonoSLAMGlow::Draw3D);
* threedtool->ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ProcessHits);
* threedtool->SetCameraParameters(Graphics_Fku, Graphics_Fkv,
* Graphics_U0, Graphics_V0);
*/
// Set start position for GL camera in 3D tool
// This is x, y, z position
Vector rthreed = new Vector(0.0f, 0.2f, -1.5f);
// This is camera orientation in my normal coordinate system
// (z forward, y up, x left)
Quaternion qWRthreed = new Quaternion(0.0f, 0.0f, 0.0f, 1.0f);
// We need to adjust by this rotation to fit GL coordinate frame
// (z backward, y up, x right)
// So rotate by pi about y axis
Quaternion qROthreed = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
Quaternion qWOthreed = qWRthreed.Multiply(qROthreed);
//threedtool.SetCameraPositionOrientation(rthreed, qWOthreed);
// Second 3D display for images and augmented reality
/*
* image_threedtool = new ThreeDToolGlowWidget(this,
* controlpanel1->Width() + controlpanel2->Width(), threedtool->Height(),
* monoslaminterface->GetCameraWidth(), monoslaminterface->GetCameraHeight());
* image_threedtool.DrawEvent.Attach(this, &MonoSLAMGlow::ImageDraw3D);
* image_threedtool.ProcessHitsEvent.Attach(this, &MonoSLAMGlow::ImageProcessHits);
* image_threedtool.SetCameraParameters(Graphics_Fku, Graphics_Fkv, Graphics_U0, Graphics_V0);
*/
// Set up initial state of virtual camera for image display to match
// state vector
Scene_Single scene = monoslaminterface.GetScene();
if (scene != null)
{
Vector v = scene.get_xv();
if (v != null)
{
scene.get_motion_model().func_xp(v);
ThreeD_Motion_Model threed_motion_model = (ThreeD_Motion_Model)scene.get_motion_model();
Vector3D r = threed_motion_model.get_rRES();
Quaternion qWR = threed_motion_model.get_qRES();
// q is qWR between world frame and Scene robot frame
// We need to adjust by this rotation to fit GL coordinate frame
// (z backward, y up, x right)
// So rotate by pi about y axis
Quaternion qRO = new Quaternion(0.0f, 1.0f, 0.0f, 0.0f);
Quaternion qWO = qWR.Multiply(qRO);
//image_threedtool.SetCameraPositionOrientation(r, qWO);
}
else
{
Debug.WriteLine("Scene xp not defined");
}
}
else
{
Debug.WriteLine("No scene object defined");
}
}
else
{
Debug.WriteLine("No partially initialised feature measurement model found");
}
}