00001 // Copyright 2008 Isis Innovation Limited 00002 #include "MapPoint.h" 00003 #include "KeyFrame.h" 00004 void MapPoint::RefreshPixelVectors() 00005 { 00006 KeyFrame &k = *pPatchSourceKF; 00007 00008 // Find patch pos in KF camera coords 00009 // Actually this might not exactly correspond to the patch pos! 00010 // Treat it as a general point on the plane. 00011 Vector<3> v3PlanePoint_C = k.se3CfromW * v3WorldPos; 00012 00013 // Find the height of this above the plane. 00014 // Assumes the normal is pointing toward the camera. 00015 double dCamHeight = fabs((double)(v3PlanePoint_C * v3Normal_NC)); 00016 00017 double dPixelRate = fabs((double)(v3Center_NC * v3Normal_NC)); 00018 double dOneRightRate = fabs((double)(v3OneRightFromCenter_NC * v3Normal_NC)); 00019 double dOneDownRate = fabs((double)(v3OneDownFromCenter_NC * v3Normal_NC)); 00020 00021 // Find projections onto plane 00022 Vector<3> v3CenterOnPlane_C = v3Center_NC * dCamHeight / dPixelRate; 00023 Vector<3> v3OneRightOnPlane_C = v3OneRightFromCenter_NC * dCamHeight / dOneRightRate; 00024 Vector<3> v3OneDownOnPlane_C = v3OneDownFromCenter_NC * dCamHeight / dOneDownRate; 00025 00026 // Find differences of these projections in the world frame 00027 v3PixelRight_W = k.se3CfromW.get_rotation().inverse() * (v3OneRightOnPlane_C - v3CenterOnPlane_C); 00028 v3PixelDown_W = k.se3CfromW.get_rotation().inverse() * (v3OneDownOnPlane_C - v3CenterOnPlane_C); 00029 }